Example #1
0
int soclcp_compute_error_v(SecondOrderConeLinearComplementarityProblem* problem, double *z , double *w, double tolerance, SolverOptions *options, double * error)
{
  /* Checks inputs */
  if(problem == NULL || z == NULL || w == NULL)
    numerics_error("soclcp_compute_error", "null input for problem and/or z and/or w");

  /* Computes w = Mz + q */
  int incx = 1, incy = 1;
  int nc = problem->nc;
  int n = problem->n;
  double *mu = problem->tau;

  double invmu = 0.0;
  cblas_dcopy(n , problem->q , incx , z , incy); // z <-q

  // Compute the current reaction
  NM_gemv(1.0, problem->M, w, 1.0, z);

  *error = 0.;
  double rho = 1.0;
  for(int ic = 0 ; ic < nc ; ic++)
  {
    int dim = problem->coneIndex[ic+1]-problem->coneIndex[ic];
    double * worktmp = (double *)malloc(dim*sizeof(double)) ;
    int nic = problem->coneIndex[ic];
    for (int i=0; i < dim; i++)
    {
      worktmp[i] = w[nic+i] - rho * z[nic+i];
    }
    invmu = 1.0 / mu[ic];
    projectionOnSecondOrderCone(worktmp, invmu, dim);
    for (int i=0; i < dim; i++)
    {
      worktmp[i] = w[nic+i] - worktmp[i];
      *error +=  worktmp[i] * worktmp[i];
    }
    free(worktmp);
  }
  *error = sqrt(*error);

  /* Computes error */
  double norm_q = cblas_dnrm2(n , problem->q , incx);
  *error = *error / (norm_q + 1.0);
  if(*error > tolerance)
  {
    /*      if (verbose > 0) printf(" Numerics - soclcp_compute_error_velocity failed: error = %g > tolerance = %g.\n",*error, tolerance); */
    return 1;
  }
  else
    return 0;
}
Example #2
0
void fc3d_nsgs_fillMLocal(FrictionContactProblem * problem, FrictionContactProblem * localproblem, int contact)
{

  NumericsMatrix * MGlobal = problem->M;

  int n = 3 * problem->numberOfContacts;


  int storageType = MGlobal->storageType;
  if (storageType == 0)
    // Dense storage
  {
    int in = 3 * contact, it = in + 1, is = it + 1;
    int inc = n * in;
    double * MM = MGlobal->matrix0;
    double * MLocal =  localproblem->M->matrix0;

    /* The part of MM which corresponds to the current block is copied into MLocal */
    MLocal[0] = MM[inc + in];
    MLocal[1] = MM[inc + it];
    MLocal[2] = MM[inc + is];
    inc += n;
    MLocal[3] = MM[inc + in];
    MLocal[4] = MM[inc + it];
    MLocal[5] = MM[inc + is];
    inc += n;
    MLocal[6] = MM[inc + in];
    MLocal[7] = MM[inc + it];
    MLocal[8] = MM[inc + is];
  }
  else if (storageType == 1)
  {
    int diagPos = getDiagonalBlockPos(MGlobal->matrix1, contact);
    localproblem->M->matrix0 = MGlobal->matrix1->block[diagPos];

  }
  else
    numerics_error("fc3d_projection -", "unknown storage type for matrix M");

}
Example #3
0
int lcp_compute_error(LinearComplementarityProblem* problem, double *z , double *w, double tolerance, double * error)
{
  /* Checks inputs */
  if (problem == NULL || z == NULL || w == NULL)
    numerics_error("lcp_compute_error", "null input for problem and/or z and/or w");

  /* Computes w = Mz + q */
  int incx = 1, incy = 1;
  unsigned int n = problem->size;
  cblas_dcopy(n , problem->q , incx , w , incy);  // w <-q
  prodNumericsMatrix(n, n, 1.0, problem->M, z, 1.0, w);
  double normq = cblas_dnrm2(n , problem->q , incx);
  lcp_compute_error_only(n, z, w, error);
  *error = *error / (normq + 1.0); /* Need some comments on why this is needed */
  if (*error > tolerance)
  {
    if (verbose > 0) printf(" Numerics - lcp_compute_error : error = %g > tolerance = %g.\n", *error, tolerance);
    return 1;
  }
  else
    return 0;

}
int variationalInequality_setDefaultSolverOptions(SolverOptions* options, int solverId)
{
  solver_options_nullify(options);

  int info = -1;
  switch (solverId)
  {
  case SICONOS_VI_EG:
  {
    info =    variationalInequality_ExtraGradient_setDefaultSolverOptions(options);
    break;
  }
  case SICONOS_VI_FPP:
  {
    info =    variationalInequality_FixedPointProjection_setDefaultSolverOptions(options);
    break;
  }
  case SICONOS_VI_HP:
  {
    info =    variationalInequality_HyperplaneProjection_setDefaultSolverOptions(options);
    break;
  }
  case SICONOS_VI_BOX_QI:
  {
    info =    variationalInequality_common_setDefaultSolverOptions(options, solverId);
    break;
  }
  default:
  {
    numerics_error("variationalInequality_setDefaultSolverOptions", "Unknown Solver");

  }
  }

  return info;
}
Example #5
0
int soclcp_driver(SecondOrderConeLinearComplementarityProblem* problem,
                  double *r, double *v,
                  SolverOptions* options)
{
  if(options == NULL)
    numerics_error("soclcp_driver", "null input for solver and/or global options");

  assert(options->isSet);

  if(verbose > 0)
    solver_options_print(options);

  /* Solver name */
  /*const char* const  name = options->solverName;*/

  int info = -1 ;

  /* Check for trivial case */
  info = soclcp_checkTrivialCase(problem, v, r, options);


  if(info == 0)
    return info;


  switch(options->solverId)
  {
  /* Non Smooth Gauss Seidel (NSGS) */
  case SICONOS_SOCLCP_NSGS:
  {
    numerics_printf(" ========================== Call NSGS solver for Second Order Cone LCP problem ==========================\n");
    soclcp_nsgs(problem, r , v , &info , options);
    break;
  }
  /* case SICONOS_SOCLCP_NSGSV: */
  /* { */
  /*   numerics_printf(numerics_printf(" ========================== Call NSGSV solver for Second Order Cone LCP problem ==========================\n"); */
  /*   soclcp_nsgs_v(problem, r , v , &info , options); */
  /*   break; */
  /* } */
  /* /\* Proximal point algorithm *\/ */
  /* case SICONOS_SOCLCP_PROX: */
  /* { */
  /*   numerics_printf(numerics_printf(" ========================== Call PROX (Proximal Point) solver for Second Order Cone LCP problem ==========================\n"); */
  /*   soclcp_proximal(problem, r , v , &info , options); */
  /*   break; */
  /* } */
  /* /\* Tresca Fixed point algorithm *\/ */
  /* case SICONOS_SOCLCP_TFP: */
  /* { */
  /*   numerics_printf(" ========================== Call TFP (Tresca Fixed Point) solver for Second Order Cone LCP problem ==========================\n"); */
  /*   soclcp_TrescaFixedPoint(problem, r , v , &info , options); */
  /*   break; */
  /* } */
  case SICONOS_SOCLCP_VI_FPP:
  {
    numerics_printf(" ========================== Call VI_FixedPointProjection (VI_FPP) solver for Second Order Cone LCP problem ==========================\n");
    soclcp_VI_FixedPointProjection(problem, r , v , &info , options);
    break;
  }
  /* VI Extra Gradient algorithm */
  case SICONOS_SOCLCP_VI_EG:
  {
    numerics_printf(" ========================== Call VI_ExtraGradient (VI_EG) solver for Second Order Cone LCP problem ==========================\n");
    soclcp_VI_ExtraGradient(problem, r , v , &info , options);
    break;
  }
  /* /\* Hyperplane Projection algorithm *\/ */
  /* case SICONOS_SOCLCP_HP: */
  /* { */
  /*   numerics_printf(" ========================== Call Hyperplane Projection (HP) solver for Second Order Cone LCP problem ==========================\n"); */
  /*   soclcp_HyperplaneProjection(problem, r , v , &info , options); */
  /*   break; */
  /* } */
  /* /\* Alart Curnier in local coordinates *\/ */
  /* case SICONOS_SOCLCP_NSN_AC: */
  /* { */
  /*   numerics_printf(" ========================== Call Alart Curnier solver for Second Order Cone LCP problem ==========================\n"); */
  /*   if (problem->M->matrix0) */
  /*   { */
  /*     soclcp_nonsmooth_Newton_AlartCurnier(problem, r , v , &info , options); */
  /*   } */
  /*   else */
  /*   { */
  /*     soclcp_nonsmooth_Newton_AlartCurnier(problem, r , v , &info , options); */
  /*   } */
  /*   break; */
  /* } */
  /* /\* Fischer Burmeister in local coordinates *\/ */
  /* case SICONOS_SOCLCP_NSN_FB: */
  /* { */
  /*   numerics_printf(" ========================== Call Fischer Burmeister solver for Second Order Cone LCP problem ==========================\n"); */
  /*   soclcp_nonsmooth_Newton_FischerBurmeister(problem, r , v , &info , options); */
  /*   break; */
  /* } */
  /* case SICONOS_SOCLCP_QUARTIC_NU: */
  /* case SICONOS_SOCLCP_QUARTIC: */
  /* { */
  /*   numerics_printf(" ========================== Call Quartic solver for Second Order Cone LCP problem ==========================\n"); */
  /*   soclcp_unitary_enumerative(problem, r , v , &info , options); */
  /*   break; */
  /* } */
  /* case SICONOS_SOCLCP_AlartCurnierNewton: */
  /* case SICONOS_SOCLCP_DampedAlartCurnierNewton: */
  /* { */
  /*   numerics_printf(" ========================== Call Quartic solver for Second Order Cone LCP problem ==========================\n"); */
  /*   info =soclcp_Newton_solve(problem, r , options); */
  /*   break; */
  /* } */
  default:
  {
    fprintf(stderr, "Numerics, SecondOrderConeLinearComplementarity_driver failed. Unknown solver.\n");
    exit(EXIT_FAILURE);

  }
  }

  return info;

}
int mixedLinearComplementarity_setDefaultSolverOptions(MixedLinearComplementarityProblem* problem, SolverOptions* pOptions)
{
  solver_options_nullify(pOptions);
  int info = -1;

  switch (pOptions->solverId)
  {
  case SICONOS_MLCP_DIRECT_ENUM:
  {
    info =    mixedLinearComplementarity_directEnum_setDefaultSolverOptions(problem, pOptions);
    break;
  }
  case SICONOS_MLCP_PATH_ENUM:
  {
    info =    mixedLinearComplementarity_pathEnum_setDefaultSolverOptions(problem, pOptions);
    break;
  }
  case  SICONOS_MLCP_DIRECT_PATH_ENUM:
  {
    info =    mixedLinearComplementarity_directPathEnum_setDefaultSolverOptions(problem, pOptions);
    break;
  }
  case SICONOS_MLCP_DIRECT_SIMPLEX:
  {
    info =    mixedLinearComplementarity_directSimplex_setDefaultSolverOptions(problem, pOptions);
    break;
  }
  case SICONOS_MLCP_DIRECT_PATH:
  {
    info =    mixedLinearComplementarity_directPath_setDefaultSolverOptions(problem, pOptions);
    break;
  }
  case SICONOS_MLCP_DIRECT_FB:
  {
    info =    mixedLinearComplementarity_directFB_setDefaultSolverOptions(problem, pOptions);
    break;
  }
  case SICONOS_MLCP_SIMPLEX:
  {
    info =    mixedLinearComplementarity_simplex_setDefaultSolverOptions(problem, pOptions);
    break;
  }
  case SICONOS_MLCP_PGS:
  {
    info =    mixedLinearComplementarity_pgs_setDefaultSolverOptions(problem, pOptions);
    break;
  }
  case SICONOS_MLCP_PGS_SBM:
  {
    info =    mixedLinearComplementarity_pgs_SBM_setDefaultSolverOptions(problem, pOptions);
    break;
  }
  case SICONOS_MLCP_RPGS:
  {
    info =    mixedLinearComplementarity_rpgs_setDefaultSolverOptions(problem, pOptions);
    break;
  }
  case SICONOS_MLCP_RPSOR:
  {
    info =    mixedLinearComplementarity_rpsor_setDefaultSolverOptions(problem, pOptions);
    break;
  }
  case SICONOS_MLCP_PATH:
  {
    info =    mixedLinearComplementarity_path_setDefaultSolverOptions(problem, pOptions);
    break;
  }
  case SICONOS_MLCP_ENUM:
  {
    info =    mixedLinearComplementarity_enum_setDefaultSolverOptions(problem, pOptions);
    break;
  }
  case SICONOS_MLCP_FB:
  {
    info =    mixedLinearComplementarity_fb_setDefaultSolverOptions(problem, pOptions);
    break;
  }
  case SICONOS_MLCP_PSOR:
  {
    info = mixedLinearComplementarity_psor_setDefaultSolverOptions(problem, pOptions);
    break;
  }
  default:
  {
    numerics_error("mixedLinearComplementarity_setDefaultSolverOptions", "Unknown Solver");

  }
  }
  return info;
}
Example #7
0
int fc2d_compute_error(FrictionContactProblem* problem, double *z , double *w, double tolerance, double * error)
{

  /* Checks inputs */
  if (! problem || ! z || ! w)
    numerics_error("fc2d_compute_error", "null input for problem and/or z and/or w");

  int nc = problem->numberOfContacts;

  int n = nc * 2;

  int ic, iN, iT;

  double *mu = problem->mu;

  double tmp[2];

  double normT;

  cblas_dcopy(n, problem->q, 1, w, 1); // w <-q
  prodNumericsMatrix(n, n, 1.0, problem->M, z, 1.0, w);

  *error = 0.;

  /* Num. Methods For Nonsmooth Dynamics, A.13 P 528 */
  /* DesaxceFeng98 */
  /* K* -) x _|_ y  (- K  <=>  x = projK(x-rho.y) for all rho>0 */

  for (ic = 0, iN = 0, iT = 1 ; ic < nc ; ++ic , ++iN, ++iN, ++iT, ++iT)
  {
    /* Compute the modified local velocity */
    tmp[0] = z[iN] - (w[iN] + mu[ic] * fabs(w[iT]));  /* rho=1 */
    tmp[1] = z[iT] - w[iT];                     /* rho=1 */

    /* projection */
    normT = fabs(tmp[1]);
    if (mu[ic]*normT <= -tmp[0])
    {
      tmp[0] = 0.;
      tmp[1] = 0.;
    }
    else if (normT > mu[ic]*tmp[0])
    {
      /* solve([sqrt((r1-mu*ra)^2+(r0-ra)^2)=abs(mu*r0-r1)/sqrt(mu*mu+1)],[ra]) */
      tmp[0] = (mu[ic] * normT + tmp[0]) / (mu[ic] * mu[ic] + 1);
      tmp[1] = mu[ic] * tmp[0] * SGN(tmp[1]);
    }

    tmp[0] = z[iN] -  tmp[0];
    tmp[1] = z[iT] -  tmp[1];
    *error += tmp[0] * tmp[0] + tmp[1] * tmp[1];

  }

  *error = sqrt(*error);
  *error /= (cblas_dnrm2(n, problem->q, 1) + 1.0);

  if (*error > tolerance)
  {
    if (verbose > 1) printf(" Numerics - fc2d_compute_error failed: error = %g > tolerance = %g.\n", *error, tolerance);
    return 1;
  }
  else
    return 0;
}
Example #8
0
int fc2d_tolcp(FrictionContactProblem* problem, LinearComplementarityProblem * lcp_problem)
{
  if (problem->dimension != 2)
  {
    numerics_error("fc2d_tolcp", "Dimension of the problem : problem-> dimension is not compatible or is not set");
    return 1;
  }
  int nc = problem->numberOfContacts;
  lcp_problem->size = 3 * nc ;
  lcp_problem->M = newNumericsMatrix();
  lcp_problem->M->size0 = 3 * nc ;
  lcp_problem->M->size1 = 3 * nc ;

  lcp_problem->M->storageType = 0;
  lcp_problem->M->matrix1 = NULL;
  lcp_problem->M->matrix2 = NULL;
  lcp_problem->M->internalData = NULL;
  lcp_problem->M->matrix0 = (double*)malloc(lcp_problem->size * lcp_problem->size * sizeof(double));;
  lcp_problem->q = (double*)malloc(lcp_problem->size * sizeof(double));
  int n = 2 * nc;
  int i, j;
  for (i = 0; i < nc; i++)
  {
    for (j = 0; j < nc; j++)
    {
      /* first Column */
      lcp_problem->M->matrix0[3 * i + 3 * j * lcp_problem->size] =
        problem->M->matrix0[2 * i + 2 * j * n] - problem->mu[i] * problem->M->matrix0[2 * i + (2 * j + 1) * n] ; // compute W_NN-mu W_NT

      lcp_problem->M->matrix0[3 * i + 1 + 3 * j * lcp_problem->size] =
        problem->M->matrix0[(2 * i + 1) + 2 * j * n] - problem->mu[i] * problem->M->matrix0[(2 * i + 1) + (2 * j + 1) * n] ; // compute W_TN-mu W_TT

      if (i == j)
      {
        lcp_problem->M->matrix0[3 * i + 2 + 3 * j * lcp_problem->size] = 2.0 * problem->mu[i];
      }
      else
      {
        lcp_problem->M->matrix0[3 * i + 2 + 3 * j * lcp_problem->size] = 0.0;
      }

      /* second Column */
      lcp_problem->M->matrix0[3 * i + (3 * j + 1)*lcp_problem->size] =
        problem->M->matrix0[2 * i + (2 * j + 1) * n] ; // set W_NT

      lcp_problem->M->matrix0[3 * i + 1 + (3 * j + 1)*lcp_problem->size] =
        problem->M->matrix0[(2 * i + 1) + (2 * j + 1) * n] ; // set WTT

      if (i == j)
      {
        lcp_problem->M->matrix0[3 * i + 2 + (3 * j + 1)*lcp_problem->size] =  -1.0;
      }
      else
      {
        lcp_problem->M->matrix0[3 * i + 2 + (3 * j + 1)*lcp_problem->size] =  0.0;
      }

      /* Third Column */
      lcp_problem->M->matrix0[3 * i + (3 * j + 2)*lcp_problem->size] =  0.0;


      if (i == j)
      {
        lcp_problem->M->matrix0[3 * i + 1 + (3 * j + 2)*lcp_problem->size] =  1.0;
      }
      else
      {
        lcp_problem->M->matrix0[3 * i + 1 + (3 * j + 2)*lcp_problem->size] =  0.0;
      }

      lcp_problem->M->matrix0[3 * i + 2 + (3 * j + 2)*lcp_problem->size] =  0.0;

    }
  }


  for (i = 0; i < nc; i++)
  {
    lcp_problem->q[3 * i  ] = problem->q[2 * i];
    lcp_problem->q[3 * i + 1] = problem->q[2 * i + 1];
    lcp_problem->q[3 * i + 2] = 0.0;;
  }

  return 0;

}
Example #9
0
int extractLCP(NumericsMatrix* MGlobal, double *z , int *indic, int *indicop, double *submatlcp , double *submatlcpop,
               int *ipiv , int *sizesublcp , int *sizesublcpop)
{
  if (MGlobal == NULL || z == NULL)
    numerics_error("extractLCP", "Null input for one arg (problem, z, ...)");

  int info;
  /*  double epsdiag = DBL_EPSILON;*/

  /* Extract data from problem */
  if (MGlobal->storageType == 1)
    numerics_error("extractLCP", "Not yet implemented for sparse storage");
  double * M = MGlobal->matrix0;
  int sizelcp = MGlobal->size0;
  if (M == NULL)
    numerics_error("extractLCP", "Null input matrix M");

  /*  workspace = (double*)malloc(sizelcp * sizeof(double)); */
  /*    printf("recalcul_submat\n");*/


  /* indic = set of indices for which z[i] is positive */
  /* indicop = set of indices for which z[i] is null */

  /* test z[i] sign */
  int i, j = 0, k = 0;
  for (i = 0; i < sizelcp; i++)
  {
    if (z[i] > w[i]) /* if (z[i] >= epsdiag)*/
    {
      indic[j] = i;
      j++;
    }
    else
    {
      indicop[k] = i;
      k++;
    }
  }

  /* size of the sub-matrix that corresponds to indic */
  *sizesublcp = j;
  /* size of the sub-matrix that corresponds to indicop */
  *sizesublcpop = k;

  /* If indic is non-empty, copy corresponding M sub-matrix into submatlcp */
  if (*sizesublcp != 0)
  {
    for (j = 0; j < *sizesublcp; j++)
    {
      for (i = 0; i < *sizesublcp; i++)
        submatlcp[(j * (*sizesublcp)) + i] = M[(indic[j] * sizelcp) + indic[i]];
    }

    /* LU factorization and inverse in place for submatlcp */
    DGETRF(*sizesublcp, *sizesublcp, submatlcp, *sizesublcp, ipiv, info);
    if (info != 0)
    {
      numerics_warning("extractLCP", "LU factorization failed") ;
      return 1;
    }

    DGETRI(*sizesublcp, submatlcp, *sizesublcp, ipiv , info);
    if (info != 0)
    {
      numerics_warning("extractLCP", "LU inversion failed");
      return 1;
    }

    /* if indicop is not empty, copy corresponding M sub-matrix into submatlcpop */
    if (*sizesublcpop != 0)
    {
      for (j = 0; j < *sizesublcp; j++)
      {
        for (i = 0; i < *sizesublcpop; i++)
          submatlcpop[(j * (*sizesublcpop)) + i] = vec[(indic[j] * sizelcp) + indicop[i]];
      }
    }
  }

  return 0;
}
Example #10
0
int predictLCP(int sizeLCP, double* q, double *z , double *w , double tol,
               int *indic , int *indicop , double *submatlcp , double *submatlcpop ,
               int *ipiv , int *sizesublcp , int *sizesublcpop , double *subq , double *bufz , double *newz)
{
  if (q == NULL ||  z == NULL || w == NULL)
    numerics_error("predictLCP", "Null input for one arg (problem, q,w ...)");

  int i, sizelcp, info, incx;
  double error, normq;
  double zi, wi;
  int incx = 1;

  /* Copy of z into a buffer for restart if predict failed */
  cblas_dcopy(sizeLCP, z , incx , bufz , incx);   /* Saving z on enter. */

  /* Sets z and w to 0*/
  for (i = 0; i < sizeLCP; i++)
  {
    z[i] = 0.;
    w[i] = 0.;
  }

  /* if indic is not empty, computes solution of newz of submatlcp.newz = subq */
  if (*sizesublcp != 0)
  {
    /* Gets subq */
    for (i = 0; i < *sizesublcp; i++)
      subq[i] = -q[indic[i]];


    cblas_dgemv(CblasColMajor,CblasNoTrans, *sizesublcp , *sizesublcp , 1.0 , submatlcp , *sizesublcp , subq , incx , 0.0 , newz , incx);

    /* Copy of newz into z for i in indic */
    for (i = 0; i < *sizesublcp; i++)
    {
      /*        z[indic[i]] = subq[i];*/
      /*        if (newz[i] > 0) z[indic[i]] = newz[i];*/
      z[indic[i]] = newz[i];
    }
  }

  /* if indicop is not empty, computes subw = submatlcpop.newz + subq - subw saved in subq */
  if (*sizesublcpop != 0)
  {
    /* Gets subq */
    for (i = 0; i < *sizesublcpop; i++)
      subq[i] = q[indicop[i]];

    if (*sizesublcp != 0)
      cblas_dgemv(CblasColMajor,CblasNoTrans, *sizesublcpop , *sizesublcp , 1.0, submatlcpop, *sizesublcpop, newz, incx, 1.0, subq, incx);

    /* Copy of subq=subw into w for indices in indicop */
    for (i = 0; i < *sizesublcpop; i++)
      w[indicop[i]] = subq[i];
  }

  /* Error evaluation */
  error = 0.;
  for (i = 0 ; i < sizeLCP ; i++)
  {
    zi = z[i];
    wi = w[i];
    if (zi < 0.0)
    {
      error += -zi;
      if (wi < 0.0) error += zi * wi;
    }
    if (wi < 0.0) error += -wi;
    if ((zi > 0.0) && (wi > 0.0)) error += zi * wi;
  }

  normq = cblas_dnrm2(sizeLCP, q, incx);
  error = error / normq;

  if (error > tol)
  {
    printf("Numerics warning - predictLCP failed, error = %g > tolerance = %g - Reset z to starting value.\n", error, tol);
    info = -1;
  }
  else info = *sizesublcp;

  /* If failed, reset z to starting value (saved in bufz) */
  if (info < 0)
    cblas_dcopy(sizeLCP , bufz , incx, z, incx);

  return info;

}
void fc3d_nonsmooth_Newton_solvers_solve(fc3d_nonsmooth_Newton_solvers* equation,
                                      double* reaction,
                                      double* velocity,
                                      int* info,
                                      SolverOptions* options)
{


  assert(equation);

  FrictionContactProblem* problem = equation->problem;

  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->M->matrix0 || problem->M->matrix1 || problem->M->matrix2);

  assert(!options->iparam[4]); // only host

  unsigned int problemSize = 3 * problem->numberOfContacts;

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

  int nzmax;

  if (problem->M->storageType == NM_DENSE)
  {
    nzmax = problemSize * problemSize;
  }
  else
  {
    nzmax = options->iparam[3];
  }

  assert(itermax > 0);
  assert(nzmax > 0);

  double tolerance = options->dparam[0];
  assert(tolerance > 0);
  
  if (verbose > 0)
    printf("------------------------ FC3D - _nonsmooth_Newton_solversSolve - Start with tolerance = %g\n", tolerance);

  unsigned int _3problemSize = 3 * problemSize;
  double normq = cblas_dnrm2(problemSize , problem->q , 1);
 
  void *buffer;

  if (!options->dWork)
  {
    buffer = malloc((11 * problemSize) * sizeof(double)); // F(1),
                                                          // tmp1(1),
                                                          // tmp2(1),
                                                          // tmp3(1),
                                                          // A(3),
                                                          // B(3), rho
  }
  else
  {
    buffer = options->dWork;
  }
  double *F = (double *) buffer;
  double *tmp1 = (double *) F + problemSize;
  double *tmp2 = (double *) tmp1 + problemSize;
  double *tmp3 = (double *) tmp2 + problemSize;
  double *Ax = tmp3 + problemSize;
  double *Bx = Ax + _3problemSize;
  double *rho = Bx + _3problemSize;

  NumericsMatrix *AWpB, *AWpB_backup;
  if (!options->dWork)
  {
    AWpB = createNumericsMatrix(problem->M->storageType,
        problem->M->size0, problem->M->size1);

    AWpB_backup = createNumericsMatrix(problem->M->storageType,
        problem->M->size0, problem->M->size1);
  }
  else
  {
    AWpB = (NumericsMatrix*) (rho + problemSize);
    AWpB_backup = (NumericsMatrix*) (AWpB + sizeof(NumericsMatrix*));
  }

  /* just for allocations */
  NM_copy(problem->M, AWpB);

  if (problem->M->storageType != NM_DENSE)
  {
    switch(options->iparam[13])
    {
      case 0:
        {
          NM_linearSolverParams(AWpB)->solver = NS_CS_LUSOL;
          break;
        }
      case 1:
        {
          NM_linearSolverParams(AWpB)->solver = NS_MUMPS;

#ifdef HAVE_MPI

          assert (options->solverData);

          if ((MPI_Comm) options->solverData == MPI_COMM_NULL)
          {
            options->solverData = NM_MPI_com(MPI_COMM_NULL);
          }
          else
          {
            NM_MPI_com((MPI_Comm) options->solverData);
          }

#endif
          break;
        }
      default:
        {
          numerics_error("fc3d_nonsmooth_Newton_solvers_solve", "Unknown linear solver.\n");
        }
    }
  }

  // compute rho here
  for (unsigned int i = 0; i < problemSize; ++i) rho[i] = options->dparam[3];

  // velocity <- M*reaction + qfree
  cblas_dcopy(problemSize, problem->q, 1, velocity, 1);
  NM_gemv(1., problem->M, reaction, 1., velocity);
  
  double linear_solver_residual=0.0;
  
  while (iter++ < itermax)
  {

    equation->function(equation->data,
                       problemSize,
                       reaction, velocity, equation->problem->mu,
                       rho,
                       F, Ax, Bx);

    // AW + B
    computeAWpB(Ax, problem->M, Bx, AWpB);

    cblas_dcopy_msan(problemSize, F, 1, tmp1, 1);
    cblas_dscal(problemSize, -1., tmp1, 1);

    /* Solve: AWpB X = -F */
    NM_copy(AWpB, AWpB_backup);
    int lsi = NM_gesv(AWpB, tmp1);

    /* NM_copy needed here */
    NM_copy(AWpB_backup, AWpB);

    if (lsi)
    {
      if (verbose > 0)
      {
        numerics_warning("fc3d_nonsmooth_Newton_solvers_solve",
                         "warning! linear solver exit with code = %d\n", lsi);
      }
    }

    if (verbose > 0)
    {
      cblas_dcopy_msan(problemSize, F, 1, tmp3, 1);
      NM_gemv(1., AWpB, tmp1, 1., tmp3);
      linear_solver_residual = cblas_dnrm2(problemSize, tmp3, 1);
      /* fprintf(stderr, "fc3d esolve: linear equation residual = %g\n", */
      /*         cblas_dnrm2(problemSize, tmp3, 1)); */
      /* for the component wise scaled residual: cf mumps &
       * http://www.netlib.org/lapack/lug/node81.html */
    }
    // line search
    double alpha = 1;
    int info_ls = 0;

    cblas_dcopy_msan(problemSize, tmp1, 1, tmp3, 1);

    switch (options->iparam[11])
    {
    case -1:
      /* without line search */
      info_ls = 1;
      break;

    case 0:
      /* Goldstein Price */
      info_ls = globalLineSearchGP(equation, reaction, velocity, problem->mu, rho, F, Ax, Bx, problem->M, problem->q, AWpB, tmp1, tmp2, &alpha, options->iparam[12]);
      break;
    case 1:
      /* FBLSA */
      info_ls = frictionContactFBLSA(equation, reaction, velocity, problem->mu, rho, F, Ax, Bx,
                                     problem->M, problem->q, AWpB, tmp1, tmp2, &alpha, options->iparam[12]);
      break;
    default:
      {
        numerics_error("fc3d_nonsmooth_Newton_solvers_solve",
                       "Unknown line search option.\n");
      }
    }

    if (!info_ls)
      // tmp2 should contains the reaction iterate of the line search
      //  for GP this should be the same as cblas_daxpy(problemSize, alpha, tmp1, 1, reaction, 1);
      cblas_dcopy(problemSize, tmp2, 1, reaction, 1);
    else
      cblas_daxpy(problemSize, 1., tmp3, 1., reaction, 1);


    // velocity <- M*reaction + qfree
    cblas_dcopy(problemSize, problem->q, 1, velocity, 1);
    NM_gemv(1., problem->M, reaction, 1., velocity);

    options->dparam[1] = INFINITY;

    if (!(iter % erritermax))
    {

      fc3d_compute_error(problem, reaction, velocity,
//      fc3d_FischerBurmeister_compute_error(problem, reaction, velocity,
                         tolerance, options, normq, &(options->dparam[1]));

      DEBUG_EXPR_WE(equation->function(equation->data, problemSize,
                                       reaction, velocity, equation->problem->mu, rho,
                                       F, NULL, NULL));


      DEBUG_EXPR_WE(assert((cblas_dnrm2(problemSize, F, 1)
                            / (1 + cblas_dnrm2(problemSize, problem->q, 1)))
                           <= (10 * options->dparam[1] + 1e-15)));

    }

    if (verbose > 0)
    {
      equation->function(equation->data, problemSize,
                         reaction, velocity, equation->problem->mu, rho,
                         F, NULL, NULL);

      printf("   ---- fc3d_nonsmooth_Newton_solvers_solve: iteration %d : , linear solver residual =%g, residual=%g, ||F||=%g\n", iter, linear_solver_residual, options->dparam[1],cblas_dnrm2(problemSize, F, 1));
    }

    if (options->callback)
    {
      options->callback->collectStatsIteration(options->callback->env, problemSize, reaction, velocity,
                                      options->dparam[1], NULL);
    }

    if (isnan(options->dparam[1]))
    {
       if (verbose > 0)
       {
         printf("            fc3d_nonsmooth_Newton_solvers_solve: iteration %d : computed residual is not a number, stop.\n", iter);
       }
       info[0] = 2;
       break;
    }

    if (options->dparam[1] < tolerance)
    {
      info[0] = 0;
      break;
    }

  }

  if (verbose > 0)
  {
    if (!info[0])
      printf("------------------------ FC3D - NSN - convergence after %d iterations, residual : %g < %g \n",  iter, options->dparam[1],tolerance);
    else
    {
      printf("------------------------ FC3D - NSN - no convergence after %d iterations, residual : %g  < %g \n",  iter, options->dparam[1], tolerance);
    }
  }

  options->iparam[SICONOS_IPARAM_ITER_DONE] = iter;

  if (!options->dWork)
  {
    assert(buffer);
    free(buffer);
    options->dWork = NULL;
  }
  else
  {
    assert(buffer == options->dWork);
  }

  if (!options->dWork)
  {
    freeNumericsMatrix(AWpB);
    freeNumericsMatrix(AWpB_backup);

    free(AWpB);
    free(AWpB_backup);
  }
  if (verbose > 0)
    printf("------------------------ FC3D - NSN - End\n");

}
void fc3d_Panagiotopoulos_FixedPoint(FrictionContactProblem* problem, double *reaction, double *velocity, int* info, SolverOptions* options)
{

  /* verbose=1; */
  /* int and double parameters */
  int* iparam = options->iparam;
  double* dparam = options->dparam;

  /* Number of contacts */
  int nc = problem->numberOfContacts;

  /* Maximum number of iterations */
  int itermax = iparam[SICONOS_IPARAM_MAX_ITER];
  /* Tolerance */
  double tolerance = dparam[SICONOS_DPARAM_TOL];
  double norm_q = cblas_dnrm2(nc*3 , problem->q , 1);


  if (options->numberOfInternalSolvers < 1)
  {
    numerics_error("fc3d_TrescaFixedpoint", "The Tresca Fixed Point method needs options for the internal solvers, options[0].numberOfInternalSolvers should be >1");
  }

  SolverOptions * internalsolver_options = options->internalSolvers;

  if (verbose) solver_options_print(options);

  /*****  Fixed Point Iterations *****/
  int iter = 0; /* Current iteration number */
  double error = 1.; /* Current error */
  int hasNotConverged = 1;

  normalInternalSolverPtr internalsolver_normal;
  tangentInternalSolverPtr internalsolver_tangent;
  options->dWork = (double *) malloc(nc * sizeof(double));
  options->dWorkSize = nc;
  double * mu = options->dWork;
  internalsolver_options->dWork = options->dWork;

  double * r_n = (double *) malloc(nc * sizeof(double));

  double * r_t = (double *) malloc(2* nc * sizeof(double));
  for (int contact = 0 ; contact < nc; contact ++)
  {
    r_n[contact] = reaction[contact*3];
    r_t[2*contact] = reaction[contact*3+1];
    r_t[2*contact+1] = reaction[contact*3+2];
  }

  SplittedFrictionContactProblem * splitted_problem = (SplittedFrictionContactProblem *)malloc(sizeof(SplittedFrictionContactProblem));

  createSplittedFrictionContactProblem(problem, splitted_problem);

  LinearComplementarityProblem* normal_lcp_problem;
  ConvexQP * tangent_cqp;

  if (options->numberOfInternalSolvers !=2)
    numerics_error("fc3d_Panagiotopoulos_FixedPoint", " the solver requires 2 internal solver");

  if (internalsolver_options[0].solverId == SICONOS_LCP_PGS||
      internalsolver_options[0].solverId == SICONOS_LCP_CONVEXQP_PG)
  {
 
    normal_lcp_problem = (LinearComplementarityProblem*)malloc(sizeof(LinearComplementarityProblem));
    normal_lcp_problem->size = nc;
    normal_lcp_problem->M = splitted_problem->M_nn;

    /* for (int contact = 0 ; contact < nc; contact ++) */
    /* { */
    /*   problem->mu[contact] =0.0; */
    /* } */
    /* splitted_problem->M_nn->matrix0 =(double *)malloc(splitted_problem->M_nn->size0*splitted_problem->M_nn->size1*sizeof(double)); */
    /* SBM_to_dense(splitted_problem->M_nn->matrix1, splitted_problem->M_nn->matrix0); */
    /* splitted_problem->M_nn->storageType=NM_DENSE; */

    normal_lcp_problem->q = (double *)malloc(nc*sizeof(double));
    
  }
  else
  {
    numerics_error("fc3d_Panagiotopoulos_FixedPoint", "Unknown internal solver for the normal part.");
  }
 if (internalsolver_options[1].solverId == SICONOS_CONVEXQP_PG ||
     internalsolver_options[1].solverId == SICONOS_CONVEXQP_VI_FPP||
     internalsolver_options[1].solverId == SICONOS_CONVEXQP_VI_EG)
  {
    tangent_cqp = (ConvexQP *)malloc(sizeof(ConvexQP));
    tangent_cqp->M = splitted_problem->M_tt;
    tangent_cqp->q = (double *) malloc(2* nc * sizeof(double));
    tangent_cqp->ProjectionOnC = &Projection_ConvexQP_FC3D_Disk;
    tangent_cqp->A=NULL;
    tangent_cqp->b= NULL;
    FrictionContactProblem_as_ConvexQP *fc3d_as_cqp= (FrictionContactProblem_as_ConvexQP*)malloc(sizeof(FrictionContactProblem_as_ConvexQP));
    tangent_cqp->env = fc3d_as_cqp ;
    tangent_cqp->size = nc*2;

    /*set the norm of the VI to the norm of problem->q  */
    double norm_q_t = cblas_dnrm2(nc*2 , splitted_problem->q_t , 1);
    tangent_cqp->normConvexQP= norm_q_t;
    tangent_cqp->istheNormConvexQPset=1;

    fc3d_as_cqp->cqp = tangent_cqp;
    fc3d_as_cqp->fc3d = problem;
    fc3d_as_cqp->options = options;
  }
  else
  {
    numerics_error("fc3d_Panagiotopoulos_FixedPoint", "Unknown internal solver for the tangent part.");
  }

 if (internalsolver_options[0].solverId == SICONOS_LCP_PGS)
 {
   if (verbose > 0)
     printf(" ========================== Call LCP_PGS solver for Friction-Contact 3D problem ==========================\n");
   internalsolver_normal = &lcp_pgs;
 }  
 else if (internalsolver_options[0].solverId == SICONOS_LCP_CONVEXQP_PG)
 {
   if (verbose > 0)
     printf(" ========================== Call LCP_CONVEX_QP solver for Friction-Contact 3D problem ==========================\n");
   internalsolver_normal = &lcp_ConvexQP_ProjectedGradient;
 }
 else
 {
   numerics_error("fc3d_Panagiotopoulos_FixedPoint", "Unknown internal solver for the normal part.");
 }
   

 
 if (internalsolver_options[1].solverId == SICONOS_CONVEXQP_PG)
 {
   if (verbose > 0)
     printf(" ========================== Call SICONOS_CONVEX_QP solver for Friction-Contact 3D problem ==========================\n");
   internalsolver_tangent = &convexQP_ProjectedGradient;
 }
 else if  (internalsolver_options[1].solverId == SICONOS_CONVEXQP_VI_FPP ||
           internalsolver_options[1].solverId == SICONOS_CONVEXQP_VI_EG )
 {
   if (verbose > 0)
     printf(" ========================== Call SICONOS_CONVEX_VI_FPP solver for Friction-Contact 3D problem ==========================\n");
   internalsolver_tangent = &convexQP_VI_solver;
 }

  int cumul_internal=0;
  //verbose=1;
  while ((iter < itermax) && (hasNotConverged > 0))
  {
    ++iter;


    fc3d_set_internalsolver_tolerance(problem,options,&internalsolver_options[0], error);

    /* ----------------- */
    /* normal resolution */
    /* ----------------- */

    /* compute the rhs of the normal problem */
    cblas_dcopy(nc , splitted_problem->q_n , 1 , normal_lcp_problem->q, 1);
    NM_gemv(1.0, splitted_problem->M_nt, r_t, 1.0, normal_lcp_problem->q);

    (*internalsolver_normal)(normal_lcp_problem, r_n , velocity , info , &internalsolver_options[0]);
    cumul_internal += internalsolver_options[0].iparam[SICONOS_IPARAM_ITER_DONE];

    for (int contact = 0 ; contact < nc; contact ++)
    {
      reaction[contact*3]= r_n[contact];
    }

    fc3d_compute_error(problem, reaction , velocity, tolerance, options, norm_q,  &error);

    if (error < tolerance)
    {
      hasNotConverged = 0;
    }
    else
    {

      /* ------------------ */
      /* tangent resolution */
      /* ------------------ */

      fc3d_set_internalsolver_tolerance(problem,options,&internalsolver_options[1], error);
      /* compute the rhs of the tangent problem */
      cblas_dcopy(2*nc , splitted_problem->q_t, 1 , tangent_cqp->q, 1);
      NM_gemv(1.0, splitted_problem->M_tn, r_n, 1.0, tangent_cqp->q);

      /* Compute the value of the initial value friction threshold*/
      for (int ic = 0 ; ic < nc ; ic++) mu[ic] = fmax(0.0, problem->mu[ic] *  reaction [ic * 3]);

      /* if (verbose>0) */
      /*   printf("norm of mu = %10.5e \n", cblas_dnrm2(nc , mu , 1)); */
      fc3d_compute_error(problem, reaction , velocity, tolerance, options, norm_q,  &error);

      (*internalsolver_tangent)(tangent_cqp, r_t , velocity , info , &internalsolver_options[1]);
      cumul_internal += internalsolver_options->iparam[SICONOS_IPARAM_ITER_DONE];

      for (int contact = 0 ; contact < nc; contact ++)
      {
        reaction[contact*3+1]= r_t[2*contact];
        reaction[contact*3+2]= r_t[2*contact+1];
      }


      /* **** Criterium convergence **** */
      fc3d_compute_error(problem, reaction , velocity, tolerance, options, norm_q,  &error);


      if (options->callback)
      {
        options->callback->collectStatsIteration(options->callback->env, nc * 3,
                                                 reaction, velocity, error, NULL);
      }

      if (error < tolerance) hasNotConverged = 0;
    }



    *info = hasNotConverged;

    if (verbose > 0)
    {
      if (hasNotConverged)
      {
        printf("--------------- FC3D - PFP - Iteration %i error = %14.7e > %10.5e\n", iter, error, tolerance);
      }
      else
      {
        printf("--------------- FC3D - PFP - Iteration %i error = %14.7e < %10.5e\n", iter, error, tolerance);
        printf("--------------- FC3D - PFP - #              Internal iteration = %i\n", cumul_internal);
      }
    }
  }

  free(options->dWork);
  options->dWork = NULL;
  internalsolver_options->dWork = NULL;

  if (internalsolver_options->internalSolvers != NULL)
    internalsolver_options->internalSolvers->dWork = NULL;

  dparam[SICONOS_DPARAM_RESIDU] = error;
  iparam[SICONOS_IPARAM_ITER_DONE] = iter;

}
Example #13
0
/*
 * (input) double *z : size n+m
 * (output)double *w : size n+m
 *
 *
 */
int mlcp_compute_error(MixedLinearComplementarityProblem* problem, double *z, double *w, double tolerance, double * error)
{
  /* Checks inputs */
  if (problem == NULL || z == NULL || w == NULL)
    numerics_error("mlcp_compute_error", "null input for problem and/or z and/or w");

  int param = 1;
  int NbLines = problem->M->size0; /* Equalities */
  int n = problem->n; /* Equalities */
  int m = problem->m; /* Inequalities */
  int incx = 1, incy = 1;

  /* Computation of w: depends on the way the problem is written */

  /* Problem in the form (M,q) */
  if (problem->isStorageType1)
  {
    if (problem->M == NULL)
      numerics_error("mlcp_compute_error", "null input for M");

    /* Computes w = Mz + q */
    cblas_dcopy(NbLines , problem->q , incx , w , incy);
    prodNumericsMatrix(problem->M->size1, problem->M->size0, 1.0, problem->M, z, 1.0, w);

  }
  /* Problem in the form ABCD */
  else //if (problem->isStorageType2)
  {



    /* Checks inputs */
    if (problem->A == NULL || problem->B == NULL || problem->C == NULL  || problem->D == NULL)
    {
      numerics_error("mlcp_compute_error: ", "null input for A, B, C or D");
    }

    /* Links to problem data */
    double *a = &problem->q[0];
    double *b = &problem->q[NbLines - m];
    double *A = problem->A;
    double *B = problem->B;
    double *C = problem->C;
    double *D = problem->D;

    /* Compute "equalities" part, we = Au + Cv + a - Must be equal to 0 */
    cblas_dcopy(NbLines - m , a , incx , w , incy); //  we = w[0..n-1] <-- a
    cblas_dgemv(CblasColMajor,CblasNoTrans , NbLines - m, n , 1.0 , A , NbLines - m , &z[0] , incx , 1.0 , w , incy); // we <-- A*u + we
    cblas_dgemv(CblasColMajor,CblasNoTrans , NbLines - m, m , 1.0 , C , NbLines - m , &z[n] , incx , 1.0 , w , incy); // we <-- C*v + we

    /* Computes part which corresponds to complementarity */
    double * pwi = w + NbLines - m; // No copy!!
    cblas_dcopy(m , b , incx , pwi , incy); //  wi = w[n..m] <-- b
    // following int param, we recompute the product wi = Du+BV +b and we = Au+CV +a
    // The test is then more severe if we compute w because it checks that the linear equation is satisfied
    if (param == 1)
    {
      cblas_dgemv(CblasColMajor,CblasNoTrans , m, n , 1.0 , D , m , &z[0] , incx , 1.0 , pwi , incy);   // wi <-- D*u+ wi
      cblas_dgemv(CblasColMajor,CblasNoTrans , m , m , 1.0 , B , m , &z[n] , incx , 1.0 , pwi , incy);  // wi <-- B*v + wi
    }
  }

  /* Error on equalities part */
  double error_e = 0;
  /* Checks complementarity (only for rows number n to size) */
  double error_i = 0.;
  double zi, wi;
  double *q = problem->q;
  double norm_e = 1;
  double norm_i = 1;
  if (problem->blocksRows)
  {
    int numBlock = 0;
    while (problem->blocksRows[numBlock] < n + m)
    {
      if (!problem->blocksIsComp[numBlock])
      {
        error_e += cblas_dnrm2(problem->blocksRows[numBlock + 1] - problem->blocksRows[numBlock], w + problem->blocksRows[numBlock] , incx);
        norm_e += cblas_dnrm2(problem->blocksRows[numBlock + 1] - problem->blocksRows[numBlock], q + problem->blocksRows[numBlock] , incx);
      }
      else
      {
        for (int numLine = problem->blocksRows[numBlock]; numLine < problem->blocksRows[numBlock + 1] ; numLine++)
        {
          zi = z[numLine];
          wi = w[numLine];
          if (zi < 0.0)
          {
            error_i += -zi;
            if (wi < 0.0) error_i += zi * wi;
          }
          if (wi < 0.0) error_i += -wi;
          if ((zi > 0.0) && (wi > 0.0)) error_i += zi * wi;
        }
        norm_i += cblas_dnrm2(problem->blocksRows[numBlock + 1] - problem->blocksRows[numBlock], w + problem->blocksRows[numBlock] , incx);
      }
      numBlock++;
    }
  }
  else
  {
    printf("WARNING, DEPRECATED MLCP API\n");
    /* Error on equalities part */
    error_e = cblas_dnrm2(NbLines - m , w , incx);;

    /* Checks complementarity (only for rows number n to size) */
    error_i = 0.;

    for (int i = 0 ; i < m ; i++)
    {
      zi = z[n + i];
      wi = w[(NbLines - m) + i];
      if (zi < 0.0)
      {
        error_i += -zi;
        if (wi < 0.0) error_i += zi * wi;
      }
      if (wi < 0.0) error_i += -wi;
      if ((zi > 0.0) && (wi > 0.0)) error_i += zi * wi;
    }


    /* Computes error */
    norm_i += cblas_dnrm2(m , q + NbLines - m , incx);
    norm_e += cblas_dnrm2(NbLines - m , q , incx);
  }

  if (error_i / norm_i >= error_e / norm_e)
  {
    *error = error_i / (1.0 + norm_i);
  }
  else
  {
    *error = error_e / (1.0 + norm_e);
  }

  if (*error > tolerance)
  {
    /*if (isVerbose > 0) printf(" Numerics - mlcp_compute_error failed: error = %g > tolerance = %g.\n",*error, tolerance);*/
    if (verbose)
      printf(" Numerics - mlcp_compute_error failed: error = %g > tolerance = %g.\n", *error, tolerance);
    /* displayMLCP(problem);*/
    return 1;
  }
  else
  {
    if (verbose > 0) printf("Siconos/Numerics: mlcp_compute_error: Error evaluation = %g \n", *error);
    return 0;
  }
}
Example #14
0
void convexQP_VI_solver(ConvexQP* problem, double *z, double *w, int* info, SolverOptions* options)
{
  NumericsMatrix* A = problem->A;
  if (A)
  {
    numerics_error("ConvexQP_VI_Solver", "This solver does not support a specific matrix A different from the identity");
  }
  /* Dimension of the problem */
  int n = problem->size;

  VariationalInequality *vi = (VariationalInequality *)malloc(sizeof(VariationalInequality));

  //vi.self = &vi;
  vi->F = &Function_VI_CQP;
  vi->ProjectionOnX = &Projection_VI_CQP;

  int iter=0;
  double error=1e24;

  ConvexQP_as_VI *convexQP_as_vi= (ConvexQP_as_VI*)malloc(sizeof(ConvexQP_as_VI));
  vi->env =convexQP_as_vi ;
  vi->size =  n;

  /*set the norm of the VI to the norm of problem->q  */
  double norm_q = cblas_dnrm2(n, problem->q , 1);
  vi->normVI= norm_q;
  vi->istheNormVIset=1;

  convexQP_as_vi->vi = vi;
  convexQP_as_vi->cqp = problem;

  SolverOptions * visolver_options = (SolverOptions *) malloc(sizeof(SolverOptions));


  if (options->solverId==SICONOS_CONVEXQP_VI_FPP)
  {
    variationalInequality_setDefaultSolverOptions(visolver_options, SICONOS_VI_FPP);
  }
  else if (options->solverId==SICONOS_CONVEXQP_VI_EG)
  {
    variationalInequality_setDefaultSolverOptions(visolver_options, SICONOS_VI_EG);
  }
    
  int isize = options->iSize;
  int dsize = options->dSize;
  int vi_isize = visolver_options->iSize;
  int vi_dsize = visolver_options->dSize;
  if (isize != vi_isize )
  {
    printf("Warning: options->iSize in convexQP_VI_solver is not consitent with options->iSize in VI solver\n");
  }
  if (dsize != vi_dsize )
  {
    printf("Warning: options->iSize in convexQP_VI_solver is not consitent with options->iSize in VI solver\n");
  }
  int i;
  for (i = 0; i < min(isize,vi_isize); i++)
  {
    if (options->iparam[i] != 0 )
      visolver_options->iparam[i] = options->iparam[i] ;
  }
  for (i = 0; i <  min(dsize,vi_dsize); i++)
  {
    if (fabs(options->dparam[i]) >= 1e-24 )
      visolver_options->dparam[i] = options->dparam[i] ;
  }

  
  if (options->solverId==SICONOS_CONVEXQP_VI_FPP)
  {
    variationalInequality_FixedPointProjection(vi, z, w , info , visolver_options);
  }
  else if (options->solverId==SICONOS_CONVEXQP_VI_EG)
  {
    variationalInequality_ExtraGradient(vi, z, w , info , visolver_options);
  }


  /* **** Criterium convergence **** */
  convexQP_compute_error_reduced(problem, z , w, options->dparam[0], options, norm_q, &error);

  /* for (i =0; i< n ; i++) */
  /* { */
  /*   printf("reaction[%i]=%f\t",i,reaction[i]);    printf("velocity[%i]=F[%i]=%f\n",i,i,velocity[i]); */
  /* } */

  error = visolver_options->dparam[SICONOS_DPARAM_RESIDU];
  iter = visolver_options->iparam[SICONOS_IPARAM_ITER_DONE];

  options->dparam[SICONOS_DPARAM_RESIDU] = error;
  options->dparam[3] = visolver_options->dparam[SICONOS_VI_EG_DPARAM_RHO];
  options->iparam[SICONOS_IPARAM_ITER_DONE] = iter;


  if (verbose > 0)
  {

    if (options->solverId==SICONOS_CONVEXQP_VI_FPP)
    {
      printf("--------------- CONVEXQP - VI solver (VI_FPP) - #Iteration %i Final Residual = %14.7e\n", iter, error);
    }
    else if (options->solverId==SICONOS_CONVEXQP_VI_EG)
    {
      printf("--------------- CONVEXQP - VI solver (VI_EG) - #Iteration %i Final Residual = %14.7e\n", iter, error);
    }
    
  }
  free(vi);

  solver_options_delete(visolver_options);
  free(visolver_options);
  free(convexQP_as_vi);



}
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););

  }
void fc3d_nonsmooth_Newton_AlartCurnier(
  FrictionContactProblem* problem,
  double *reaction,
  double *velocity,
  int *info,
  SolverOptions *options)
{
  /* verbose=1; */
  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(!options->iparam[4]); // only host

  AlartCurnierParams acparams;

  switch (options->iparam[SICONOS_FRICTION_3D_NSN_FORMULATION])
  {
  case 0:
  {
    acparams.computeACFun3x3 = &computeAlartCurnierSTD;
    break;
  }
  case 1:
  {
    acparams.computeACFun3x3 = &computeAlartCurnierJeanMoreau;
    break;
  };
  case 2:
  {
    acparams.computeACFun3x3 = &fc3d_AlartCurnierFunctionGenerated;
    break;
  }
  case 3:
  {
    acparams.computeACFun3x3 = &fc3d_AlartCurnierJeanMoreauFunctionGenerated;
    break;
  }
  }

  fc3d_nonsmooth_Newton_solvers equation;

  equation.problem = problem;
  equation.data = (void *) &acparams;
  equation.function = &nonsmoothEqnAlartCurnierFun;

  if(options->iparam[SICONOS_FRICTION_3D_NSN_HYBRID_STRATEGY] ==  SICONOS_FRICTION_3D_NSN_HYBRID_STRATEGY_VI_EG_NSN)
  {
    SolverOptions * options_vi_eg =(SolverOptions *)malloc(sizeof(SolverOptions));
    fc3d_VI_ExtraGradient_setDefaultSolverOptions(options_vi_eg);
    options_vi_eg->iparam[SICONOS_IPARAM_MAX_ITER] = 50;
    options_vi_eg->dparam[SICONOS_DPARAM_TOL] = sqrt(options->dparam[0]);
    options_vi_eg->iparam[SICONOS_VI_IPARAM_ERROR_EVALUATION] = SICONOS_VI_ERROR_EVALUATION_LIGHT;
    fc3d_VI_ExtraGradient(problem, reaction , velocity , info , options_vi_eg);

    fc3d_nonsmooth_Newton_solvers_solve(&equation, reaction, velocity, info, options);
    solver_options_delete(options_vi_eg);
    free(options_vi_eg);
  }
  else if (options->iparam[SICONOS_FRICTION_3D_NSN_HYBRID_STRATEGY] ==  SICONOS_FRICTION_3D_NSN_HYBRID_STRATEGY_NO)
  {
    fc3d_nonsmooth_Newton_solvers_solve(&equation, reaction, velocity, info, options);
  }
  else
  {
    numerics_error("fc3d_nonsmooth_Newton_AlartCurnier","Unknown nsn hybrid solver");
  }
}
Example #17
0
int gfc3d_driver(GlobalFrictionContactProblem* problem, double *reaction , double *velocity, double* globalVelocity,  SolverOptions* options)
{
  assert(options->isSet);

  if (verbose > 0)
    solver_options_print(options);

  /* Solver name */
  /*  char * name = options->solverName;*/


  int info = -1 ;

  if (problem->dimension != 3)
    numerics_error("gfc3d_driver", "Dimension of the problem : problem-> dimension is not compatible or is not set");


  /* Non Smooth Gauss Seidel (NSGS) */
  switch (options->solverId)
  {
  case SICONOS_GLOBAL_FRICTION_3D_NSGS_WR:
  {
    if (verbose == 1)
      printf(" ========================== Call NSGS_WR solver with reformulation into Friction-Contact 3D problem ==========================\n");
    Global_ipiv = NULL;
    Global_MisInverse = 0;
    Global_MisLU = 0;
    gfc3d_nsgs_wr(problem, reaction , velocity, globalVelocity, &info, options);
    break;

  }
  case SICONOS_GLOBAL_FRICTION_3D_NSGSV_WR:
  {
    if (verbose == 1)
      printf(" ========================== Call NSGSV_WR solver with reformulation into Friction-Contact 3D problem ==========================\n");
    Global_ipiv = NULL;
    Global_MisInverse = 0;
    Global_MisLU = 0;
    gfc3d_nsgs_velocity_wr(problem, reaction , velocity, globalVelocity, &info, options);
    break;
  }
  case SICONOS_GLOBAL_FRICTION_3D_NSN_AC_WR:
  {
    if (verbose == 1)
      printf(" ========================== Call NSN_AC_WR solver with reformulation into Friction-Contact 3D problem ==========================\n");
    Global_ipiv = NULL;
    Global_MisInverse = 0;
    Global_MisLU = 0;
    gfc3d_globalAlartCurnier_wr(problem, reaction , velocity, globalVelocity, &info, options);
    break;

  }
  case SICONOS_GLOBAL_FRICTION_3D_PROX_WR:
  {
    if (verbose == 1)
      printf(" ========================== Call PROX_WR solver with reformulation into Friction-Contact 3D problem ==========================\n");
    Global_ipiv = NULL;
    Global_MisInverse = 0;
    Global_MisLU = 0;
    gfc3d_proximal_wr(problem, reaction , velocity, globalVelocity, &info, options);
    break;

  }
  case SICONOS_GLOBAL_FRICTION_3D_DSFP_WR:
  {
    if (verbose == 1)
      printf(" ========================== Call DSFP_WR solver with reformulation into Friction-Contact 3D problem ==========================\n");
    Global_ipiv = NULL;
    Global_MisInverse = 0;
    Global_MisLU = 0;
    gfc3d_DeSaxceFixedPoint_wr(problem, reaction , velocity, globalVelocity, &info, options);
    break;

  }
  case SICONOS_GLOBAL_FRICTION_3D_TFP_WR:
  {
    if (verbose == 1)
      printf(" ========================== Call TFP_WR solver with reformulation into Friction-Contact 3D problem ==========================\n");
    Global_ipiv = NULL;
    Global_MisInverse = 0;
    Global_MisLU = 0;
    gfc3d_TrescaFixedPoint_wr(problem, reaction , velocity, globalVelocity, &info, options);
    break;

  }
  case SICONOS_GLOBAL_FRICTION_3D_NSGS:
  {
    Global_ipiv = NULL;
    Global_MisInverse = 0;
    Global_MisLU = 0;
    gfc3d_nsgs(problem, reaction , velocity, globalVelocity, 
                                 &info , options);
    break;

  }
  case SICONOS_GLOBAL_FRICTION_3D_NSN_AC:
  {
    gfc3d_nonsmooth_Newton_AlartCurnier(problem, reaction , velocity,
                                         globalVelocity, &info , options);
    break;

  }
  case SICONOS_GLOBAL_FRICTION_3D_GAMS_PATH:
  {
    printf(" ========================== Call PATH solver via GAMS for an AVI Friction-Contact 3D problem ==========================\n");
    gfc3d_AVI_gams_path(problem, reaction , velocity, &info, options);
    break;
  }
  case SICONOS_GLOBAL_FRICTION_3D_GAMS_PATHVI:
  {
    printf(" ========================== Call PATHVI solver via GAMS for an AVI Friction-Contact 3D problem ==========================\n");
    gfc3d_AVI_gams_pathvi(problem, reaction , globalVelocity, &info, options);
    break;
  }
  default:
  {
    fprintf(stderr, "Numerics, gfc3d_driver failed. Unknown solver %d.\n", options->solverId);
    exit(EXIT_FAILURE);

  }
  }

  return info;

}
void fc3d_nonsmooth_Newton_AlartCurnier2(
  FrictionContactProblem* problem,
  double *reaction,
  double *velocity,
  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(!options->iparam[4]); // only host

  AlartCurnierParams acparams;

  switch (options->iparam[10])
  {
  case 0:
  {
    acparams.computeACFun3x3 = &computeAlartCurnierSTD;
    break;
  }
  case 1:
  {
    acparams.computeACFun3x3 = &computeAlartCurnierJeanMoreau;
    break;
  };
  case 2:
  {
    acparams.computeACFun3x3 = &fc3d_AlartCurnierFunctionGenerated;
    break;
  }
  case 3:
  {
    acparams.computeACFun3x3 = &fc3d_AlartCurnierJeanMoreauFunctionGenerated;
    break;
  }
  }

  fc3d_nonsmooth_Newton_solvers equation;

  equation.problem = problem;
  equation.data = (void *) &acparams;
  equation.function = &nonsmoothEqnAlartCurnierFun;

  /*************************************************************************
   * START NEW STUFF
   */
  size_t problemSize = problem->M->size0;
  size_t _3problemSize = problemSize + problemSize + problemSize;
  FC3D_Newton_data opaque_data;
  opaque_data.problem = problem;
  opaque_data.equation = &equation;
  opaque_data.rho = (double*)calloc(problemSize, sizeof(double));
  for (size_t i = 0; i < problemSize; ++i) opaque_data.rho[i] = 1.;
  opaque_data.Ax = (double*)calloc(_3problemSize, sizeof(double));
  opaque_data.Bx = (double*)calloc(_3problemSize, sizeof(double));
  opaque_data.normq = cblas_dnrm2(problemSize, problem->q, 1);
  opaque_data.AwpB_data_computed = false;

  functions_LSA functions_AC;
  init_lsa_functions(&functions_AC, &FC3D_compute_F, &FC3D_compute_F_merit);
  functions_AC.compute_H = &FC3D_compute_AWpB;
  functions_AC.compute_error = &FC3D_compute_error;
  functions_AC.get_set_from_problem_data = NULL;

  set_lsa_params_data(options, problem->M);
  newton_LSA_param* params = (newton_LSA_param*) options->solverParameters;
  params->check_dir_quality = false;

  options->iparam[SICONOS_IPARAM_LSA_SEARCH_CRITERION] = SICONOS_LSA_GOLDSTEIN;
//  options->iparam[SICONOS_IPARAM_LSA_SEARCH_CRITERION] = SICONOS_LSA_ARMIJO;
  /*************************************************************************
   * END NEW STUFF
   */

  if(options->iparam[SICONOS_FRICTION_3D_NSN_HYBRID_STRATEGY] ==  SICONOS_FRICTION_3D_NSN_HYBRID_STRATEGY_VI_EG_NSN)
  {
    SolverOptions * options_vi_eg =(SolverOptions *)malloc(sizeof(SolverOptions));
    fc3d_VI_ExtraGradient_setDefaultSolverOptions(options_vi_eg);
    options_vi_eg->iparam[0] = 50;
    options_vi_eg->dparam[0] = sqrt(options->dparam[0]);
    options_vi_eg->iparam[SICONOS_VI_IPARAM_ERROR_EVALUATION] = SICONOS_VI_ERROR_EVALUATION_LIGHT;
    fc3d_VI_ExtraGradient(problem, reaction , velocity , info , options_vi_eg);
    solver_options_delete(options_vi_eg);
    free(options_vi_eg);

    newton_LSA(problemSize, reaction, velocity, info, (void *)&opaque_data, options, &functions_AC);
  }
  else if (options->iparam[SICONOS_FRICTION_3D_NSN_HYBRID_STRATEGY] ==  SICONOS_FRICTION_3D_NSN_HYBRID_STRATEGY_NO)
  {
    newton_LSA(problemSize, reaction, velocity, info, (void *)&opaque_data, options, &functions_AC);
  }
  else
  {
    numerics_error("fc3d_nonsmooth_Newton_AlartCurnier","Unknown nsn hybrid solver");
  }

  free(opaque_data.rho);
  free(opaque_data.Ax);
  free(opaque_data.Bx);
}
Example #19
0
int fc3d_driver(FrictionContactProblem* problem,
		double *reaction, double *velocity,
		SolverOptions* options)
{
  if (options == NULL)
    numerics_error("fc3d_driver", "null input for solver options");

  assert(options->isSet); /* true(1) if the SolverOptions structure has been filled in else false(0) */

  if (verbose > 1)
    solver_options_print(options);

  int info = -1 ;

  if (problem->dimension != 3)
    numerics_error("fc3d_driver", "Dimension of the problem : problem-> dimension is not compatible or is not set");

  /* Check for trivial case */
  info = checkTrivialCase(problem, velocity, reaction, options);
  if (info == 0)
  {
    /* If a trivial solution is found, we set the number of iterations to 0
       and the reached acuracy to 0.0 .
       Since the indexing of parameters is non uniform, this may have side 
       effects for some solvers. The two main return parameters iparam[7] and 
       dparam[1] have to be defined and protected by means of enum*/ 
    options->iparam[7] = 0;
    options->dparam[1] = 0.0;
    goto exit;
  }


  switch (options->solverId)
  {
    /* Non Smooth Gauss Seidel (NSGS) */
  case SICONOS_FRICTION_3D_NSGS:
  {
    numerics_printf(" ========================== Call NSGS solver for Friction-Contact 3D problem ==========================\n");
    fc3d_nsgs(problem, reaction , velocity , &info , options);
    break;
  }
  case SICONOS_FRICTION_3D_NSGSV:
  {
    numerics_printf(" ========================== Call NSGSV solver for Friction-Contact 3D problem ==========================\n");
    fc3d_nsgs_velocity(problem, reaction , velocity , &info , options);
    break;
  }
  /* Proximal point algorithm */
  case SICONOS_FRICTION_3D_PROX:
  {
    numerics_printf(" ========================== Call PROX (Proximal Point) solver for Friction-Contact 3D problem ==========================\n");
    fc3d_proximal(problem, reaction , velocity , &info , options);
    break;
  }
  /* Tresca Fixed point algorithm */
  case SICONOS_FRICTION_3D_TFP:
  {
    numerics_printf(" ========================== Call TFP (Tresca Fixed Point) solver for Friction-Contact 3D problem ==========================\n");
    fc3d_TrescaFixedPoint(problem, reaction , velocity , &info , options);
    break;
  }
  /* ACLM Fixed point algorithm */
  case SICONOS_FRICTION_3D_ACLMFP:
  {
    numerics_printf(" ========================== Call ACLM (Acary Cadoux Lemarechal Malick Fixed Point) solver for Friction-Contact 3D problem ==========================\n");
    fc3d_ACLMFixedPoint(problem, reaction , velocity , &info , options);
    break;
  }
  /* SOCLCP Fixed point algorithm */
  case SICONOS_FRICTION_3D_SOCLCP:
  {
    numerics_printf(" ========================== Call SOCLCP solver for Friction-Contact 3D problem (Associated one) ==========================\n");
    fc3d_SOCLCP(problem, reaction , velocity , &info , options);
    break;
  }
  /* De Saxce Fixed point algorithm */
  case SICONOS_FRICTION_3D_DSFP:
  {
    numerics_printf(" ========================== Call DeSaxce Fixed Point (DSFP) solver for Friction-Contact 3D problem ==========================\n");
    fc3d_DeSaxceFixedPoint(problem, reaction , velocity , &info , options);
    break;
  }
  /* Fixed point projection algorithm */
  case SICONOS_FRICTION_3D_FPP:
  {
    numerics_printf(" ========================== Call Fixed Point Projection (FPP) solver for Friction-Contact 3D problem ==========================\n");
    fc3d_fixedPointProjection(problem, reaction , velocity , &info , options);
    break;
  }

  /* Extra Gradient algorithm */
  case SICONOS_FRICTION_3D_EG:
  {
    numerics_printf(" ========================== Call ExtraGradient (EG) solver for Friction-Contact 3D problem ==========================\n");
    fc3d_ExtraGradient(problem, reaction , velocity , &info , options);
    break;
  }
  /* VI Fixed Point Projection algorithm */
  case SICONOS_FRICTION_3D_VI_FPP:
  {
    numerics_printf(" ========================== Call VI_FixedPointProjection (VI_FPP) solver for Friction-Contact 3D problem ==========================\n");
    fc3d_VI_FixedPointProjection(problem, reaction , velocity , &info , options);
    break;
  }
  /* VI Extra Gradient algorithm */
  case SICONOS_FRICTION_3D_VI_EG:
  {
    numerics_printf(" ========================== Call VI_ExtraGradient (VI_EG) solver for Friction-Contact 3D problem ==========================\n");
    fc3d_VI_ExtraGradient(problem, reaction , velocity , &info , options);
    break;
  }
  /* Hyperplane Projection algorithm */
  case SICONOS_FRICTION_3D_HP:
  {
    numerics_printf(" ========================== Call Hyperplane Projection (HP) solver for Friction-Contact 3D problem ==========================\n");
    fc3d_HyperplaneProjection(problem, reaction , velocity , &info , options);
    break;
  }
  /* Alart Curnier in local coordinates */
  case SICONOS_FRICTION_3D_NSN_AC:
  {
    numerics_printf(" ========================== Call Alart Curnier solver for Friction-Contact 3D problem ==========================\n");
    if (problem->M->matrix0)
    {
      fc3d_nonsmooth_Newton_AlartCurnier(problem, reaction , velocity , &info , options);
    }
    else
    {
      fc3d_nonsmooth_Newton_AlartCurnier(problem, reaction , velocity , &info , options);
    }
    break;
  }
  /* Fischer Burmeister in local coordinates */
  case SICONOS_FRICTION_3D_NSN_FB:
  {
    numerics_printf(" ========================== Call Fischer Burmeister solver for Friction-Contact 3D problem ==========================\n");
    fc3d_nonsmooth_Newton_FischerBurmeister(problem, reaction , velocity , &info , options);
    break;
  }
  case SICONOS_FRICTION_3D_NSN_NM:
  {
    numerics_printf(" ========================== Call natural map solver for Friction-Contact 3D problem ==========================\n");
    fc3d_nonsmooth_Newton_NaturalMap(problem, reaction , velocity , &info , options);
    break;
  }
  case SICONOS_FRICTION_3D_ONECONTACT_QUARTIC_NU:
  case SICONOS_FRICTION_3D_ONECONTACT_QUARTIC:
  {
    numerics_printf(" ========================== Call Quartic solver for Friction-Contact 3D problem ==========================\n");
    fc3d_unitary_enumerative(problem, reaction , velocity , &info , options);
    break;
  }
  case SICONOS_FRICTION_3D_ONECONTACT_NSN:
  case SICONOS_FRICTION_3D_ONECONTACT_NSN_GP:
  {
    numerics_printf(" ========================== Call Newton-based solver for one contact Friction-Contact 3D problem ==========================\n");
    fc3d_onecontact_nonsmooth_Newton_solvers_initialize(problem, problem, options);
    info = fc3d_onecontact_nonsmooth_Newton_solvers_solve(problem, reaction , options);
    break;
  }
  case SICONOS_FRICTION_3D_ONECONTACT_ProjectionOnConeWithLocalIteration:
  {
    numerics_printf(" ========================== Call Projection on cone solver for one contact Friction-Contact 3D problem ==========================\n");
    fc3d_projectionOnConeWithLocalIteration_initialize(problem, problem, options);
    info = fc3d_projectionOnConeWithLocalIteration_solve(problem, reaction , options);
    fc3d_projectionOnConeWithLocalIteration_free(problem, problem, options);

    break;
  }
  case SICONOS_FRICTION_3D_GAMS_PATH:
  {
    numerics_printf(" ========================== Call PATH solver via GAMS for an AVI Friction-Contact 3D problem ==========================\n");
    fc3d_AVI_gams_path(problem, reaction , velocity, &info, options);
    break;
  }
  case SICONOS_FRICTION_3D_GAMS_PATHVI:
  {
    numerics_printf(" ========================== Call PATHVI solver via GAMS for an AVI Friction-Contact 3D problem ==========================\n");
    fc3d_AVI_gams_pathvi(problem, reaction , velocity, &info, options);
    break;
  }
  case SICONOS_FRICTION_3D_GAMS_LCP_PATH:
  {
    numerics_printf(" ========================== Call PATH solver via GAMS for an LCP-based reformulation of the AVI Friction-Contact 3D problem ==========================\n");
    fc3d_lcp_gams_path(problem, reaction , velocity, &info, options);
    break;
  }
  case SICONOS_FRICTION_3D_GAMS_LCP_PATHVI:
  {
    numerics_printf(" ========================== Call PATHVI solver via GAMS for an LCP-based reformulation of the AVI Friction-Contact 3D problem ==========================\n");
    fc3d_lcp_gams_pathvi(problem, reaction , velocity, &info, options);
    break;
  }
  default:
  {
    fprintf(stderr, "Numerics, fc3d_driver failed. Unknown solver.\n");
    exit(EXIT_FAILURE);

  }
  }

exit:

  return info;

}
int variationalInequality_driver(VariationalInequality* problem, 
                                 double *x, double *w, 
                                 SolverOptions* options)
{
  if (options == NULL)
    numerics_error("variationalInequality_driver", "null input for solver and/or global options");

  assert(options->isSet);
  if (verbose > 0)
    solver_options_print(options);

  /* Solver name */
  /*char * name = options->solverName;*/

  int info = -1 ;

  /* Check for trivial case */
  info = checkTrivialCase_vi(problem, x, w, options);
  if (info == 0){
    double error;
    variationalInequality_computeError(problem, x , w, options->dparam[0], options, &error);
    printf("variationalInequality_driver. error = %8.4e\n", error);
    return info;
  }

  switch (options->solverId)
  {
    /* Non Smooth Gauss Seidel (NSGS) */
  /* Extra Gradient algorithm */
  case SICONOS_VI_EG:
  {
    numerics_printf_verbose(1,
             " ========================== Call ExtraGradient (EG) solver for VI problem ==========================\n");
    variationalInequality_ExtraGradient(problem, x , w , &info , options);
    break;
  }
  case SICONOS_VI_FPP:
  {
    numerics_printf_verbose(1,
             " ========================== Call Fixed Point Projection (FPP) solver for VI problem ==========================\n");
    variationalInequality_FixedPointProjection(problem, x , w , &info , options);
    break;
  }
  case SICONOS_VI_HP:
  {
    numerics_printf_verbose(1,
             " ========================== Call Hyperplane Projection (HP) solver for VI problem ==========================\n");
    variationalInequality_HyperplaneProjection(problem, x , w , &info , options);
    break;
  }
  case SICONOS_VI_BOX_QI:
  {
    variationalInequality_box_newton_QiLSA(problem, x, w, &info, options);
    break;
  }
  case SICONOS_VI_BOX_AVI_LSA:
  {
    vi_box_AVI_LSA(problem, x, w, &info, options);
    break;
  }
  case SICONOS_VI_BOX_PATH:
  {
    vi_box_path(problem, x, w, &info, options);
    break;
  }
  default:
  {
    fprintf(stderr, "Numerics, variationalInequality_driver failed. Unknown solver.\n");
    exit(EXIT_FAILURE);

  }
  }

  return info;

}
int soclcp_setDefaultSolverOptions(SolverOptions* options, int solverId)
{
  options->iparam = NULL;
  options->dparam = NULL;
  solver_options_nullify(options);

  int info = -1;
  switch(solverId)
  {
  case SICONOS_SOCLCP_NSGS:
  {
    info =    soclcp_nsgs_setDefaultSolverOptions(options);
    break;
  }
  /* case SICONOS_SOCLCP_NSGSV: */
  /* { */
  /*   info =    soclcp_nsgs_velocity_setDefaultSolverOptions(options); */
  /*   break; */
  /* } */
  /* case SICONOS_SOCLCP_*/
  /* { */
  /*   info =    soclcp_proximal_setDefaultSolverOptions(options); */
  /*   break; */
  /* } */
  /* case SICONOS_SOCLCP_TFP: */
  /* { */
  /*   info =    soclcp_TrescaFixedPoint_setDefaultSolverOptions(options); */
  /*   break; */
  /* } */
  /* case SICONOS_SOCLCP_DSFP: */
  /* { */
  /*   info =    soclcp_DeSaxceFixedPoint_setDefaultSolverOptions(options); */
  /*   break; */
  /* } */
  /* case SICONOS_SOCLCP_FPP: */
  /* { */
  /*   info =    soclcp_fixedPointProjection_setDefaultSolverOptions(options); */
  /*   break; */
  /* } */
  /* case SICONOS_SOCLCP_EG: */
  /* { */
  /*   info =    soclcp_ExtraGradient_setDefaultSolverOptions(options); */
  /*   break; */
  /* } */
  case SICONOS_SOCLCP_VI_FPP:
  {
    info =    soclcp_VI_FixedPointProjection_setDefaultSolverOptions(options);
    break;
  }
  case SICONOS_SOCLCP_VI_EG:
  {
    info =    soclcp_VI_ExtraGradient_setDefaultSolverOptions(options);
    break;
  }
  /* case SICONOS_SOCLCP_HP: */
  /* { */
  /*   info =    soclcp_HyperplaneProjection_setDefaultSolverOptions(options); */
  /*   break; */
  /* } */
  /* case SICONOS_SOCLCP_NSN_AC: */
  /* { */
  /*   info =    soclcp_AlartCurnier_setDefaultSolverOptions(options); */
  /*   break; */
  /* } */
  /* case SICONOS_SOCLCP_NSN_FB: */
  /* { */
  /*   info =    soclcp_FischerBurmeister_setDefaultSolverOptions(options); */
  /*   break; */
  /* } */
  /* case SICONOS_SOCLCP_QUARTIC: */
  /* { */
  /*   info =    soclcp_unitary_enumerative_setDefaultSolverOptions(options); */
  /*   break; */
  /* } */
  /* case SICONOS_SOCLCP_QUARTIC_NU: */
  /* { */
  /*   info =    soclcp_unitary_enumerative_setDefaultSolverOptions(options); */
  /*   options->solverId = SICONOS_SOCLCP_QUARTIC_NU; */
  /*   break; */
  /* } */
  default:
  {
    numerics_error("soclcp_setDefaultSolverOptions", "Unknown Solver");

  }
  }

  return info;
}