/* compute psi function */
void ACPsi(
  GlobalFrictionContactProblem* problem,
  AlartCurnierFun3x3Ptr computeACFun3x3,
  double *globalVelocity,
  double *reaction,
  double *velocity,
  double *rho,
  double *psi)
{

  assert(problem->H->size1 == problem->dimension * problem->numberOfContacts);

  unsigned int localProblemSize = problem->H->size1;

  unsigned int ACProblemSize = sizeOfPsi(NM_triplet(problem->M),
                                         NM_triplet(problem->H));

  unsigned int globalProblemSize = problem->M->size0;


  /* psi <-
       compute -problem->M * globalVelocity + problem->H * reaction + problem->q
     ... */
  cblas_dscal(ACProblemSize, 0., psi, 1);
  cblas_dcopy(globalProblemSize, problem->q, 1, psi, 1);
  NM_gemv(1., problem->H, reaction, 1, psi);
  NM_gemv(-1., problem->M, globalVelocity, 1, psi);


  /* psi + globalProblemSize <-
     compute -velocity + trans(problem->H) * globalVelocity + problem->b
   ... */
  cblas_daxpy(localProblemSize, -1., velocity, 1, psi + globalProblemSize, 1);
  cblas_daxpy(localProblemSize, 1, problem->b, 1, psi + globalProblemSize, 1);
  NM_tgemv(1., problem->H, globalVelocity, 1, psi + globalProblemSize);



  /* compute AC function */
  fc3d_AlartCurnierFunction(localProblemSize,
                                         computeACFun3x3,
                                         reaction,
                                         velocity, problem->mu, rho,
                                         psi+globalProblemSize+
                                         problem->H->size1,
                                         NULL, NULL);

}
static void FC3D_compute_F(void* data_opaque, double* reaction, double* velocity)
{
  FrictionContactProblem* problem = ((FC3D_Newton_data*) data_opaque)->problem;
  // velocity <- M*reaction + qfree
  cblas_dcopy(problem->M->size1, problem->q, 1, velocity, 1);
  NM_gemv(1., problem->M, reaction, 1., velocity);
}
void Function_VI_SOCLCP(void * self, int n_notused, double *x, double *F)
{
  DEBUG_PRINT("Function_VI_FC3D(void * self, double *x, double *F)\n")
  VariationalInequality * vi = (VariationalInequality *) self;
  SecondOrderConeLinearComplementarityProblem_as_VI* pb = (SecondOrderConeLinearComplementarityProblem_as_VI*)vi->env;
  SecondOrderConeLinearComplementarityProblem * soclcp = pb->soclcp;
  //frictionContact_display(fc3d);

  int n =   soclcp->n;

  cblas_dcopy(n , soclcp->q , 1 , F, 1);
  NM_gemv(1.0, soclcp->M, x, 1.0, F);
}
Exemplo n.º 4
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;
}
/* compute psi function */
void ACPsi(
  GlobalFrictionContactProblem* problem,
  AlartCurnierFun3x3Ptr computeACFun3x3,
  double *globalVelocity,
  double *reaction,
  double *velocity,
  double *rho,
  double *psi)
{

  assert(problem->H->size1 == problem->dimension * problem->numberOfContacts);
  unsigned int m = problem->H->size1;
  unsigned int n = problem->M->size0;
  unsigned int problem_size = n +  2*m ;

  cblas_dscal(problem_size, 0., psi, 1);

  /* -problem->M * globalVelocity + problem->H * reaction + problem->q ==> psi  */
  cblas_dscal(problem_size, 0., psi, 1);

  cblas_dcopy(n, problem->q, 1, psi, 1);
  NM_gemv(1., problem->H, reaction, 1, psi);
  NM_gemv(-1., problem->M, globalVelocity, 1, psi);

  /* -velocity + trans(problem->H) * globalVelocity + problem->b ==> psi + n */
  cblas_daxpy(m, -1., velocity, 1, psi + n, 1);
  cblas_daxpy(m, 1, problem->b, 1, psi + n, 1);
  NM_tgemv(1., problem->H, globalVelocity, 1, psi + n);

  /* compute AC function */
  fc3d_AlartCurnierFunction(m,
                            computeACFun3x3,
                            reaction,
                            velocity, problem->mu, rho,
                            psi+n+m,
                            NULL, NULL);

}
Exemplo n.º 6
0
void fc3d_HyperplaneProjection(FrictionContactProblem* problem, double *reaction, double *velocity, int* info, SolverOptions* options)
{
  /* int and double parameters */
  int* iparam = options->iparam;
  double* dparam = options->dparam;
  /* Number of contacts */
  int nc = problem->numberOfContacts;
  double* q = problem->q;
  NumericsMatrix* M = problem->M;
  double* mu = problem->mu;
  /* Dimension of the problem */
  int n = 3 * nc;
  /* Maximum number of iterations */
  int itermax = iparam[0];
  /* Maximum number of iterations in Line--search */
  int lsitermax = iparam[1];
  /* Tolerance */
  double tolerance = dparam[0];
  double norm_q = cblas_dnrm2(nc*3 , problem->q , 1);
 




  /*****  Fixed point iterations *****/
  int iter = 0; /* Current iteration number */
  double error = 1.; /* Current error */
  int hasNotConverged = 1;
  int contact; /* Number of the current row of blocks in M */
  int nLocal = 3;
  dparam[0] = dparam[2]; // set the tolerance for the local solver
  double * velocitytmp = (double *)calloc(n, sizeof(double));
  double * reactiontmp = (double *)calloc(n, sizeof(double));
  double * reactiontmp2 = (double *)calloc(n, sizeof(double));
  double * reactiontmp3 = (double *)calloc(n, sizeof(double));

  /* double tau = 1.0; */
  double sigma = 0.99;

  /* if (dparam[3] > 0.0) */
  /* { */
  /*   tau = dparam[3]; */
  /* } */
  /* else */
  /* { */
  /*   printf("Hyperplane Projection method. tau <=0  is not well defined\n"); */
  /*   printf("Hyperplane Projection method. rho is set to 1.0\n"); */

  /* } */
  if (dparam[4] > 0.0 && dparam[4] < 1.0)
  {
    sigma = dparam[4];
  }
  else
  {
    printf("Hyperplane Projection method. 0<sigma <1  is not well defined\n");
    printf("Hyperplane Projection method. sigma is set to 0.99\n");
  }
 
  /*   double minusrho  = -1.0*rho; */
  while ((iter < itermax) && (hasNotConverged > 0))
  {
    ++iter;

    cblas_dcopy(n , q , 1 , velocitytmp, 1);
    cblas_dcopy(n , reaction , 1 , reactiontmp, 1);

    NM_gemv(1.0, M, reactiontmp, 1.0, velocitytmp);


    // projection for each contact

    double rho = 1;

    for (contact = 0 ; contact < nc ; ++contact)
    {
      int pos = contact * nLocal;
      double  normUT = sqrt(velocitytmp[pos + 1] * velocitytmp[pos + 1] + velocitytmp[pos + 2] * velocitytmp[pos + 2]);
      reactiontmp[pos] -= rho * (velocitytmp[pos] + mu[contact] * normUT);
      reactiontmp[pos + 1] -= rho * velocitytmp[pos + 1];
      reactiontmp[pos + 2] -= rho * velocitytmp[pos + 2];
      projectionOnCone(&reactiontmp[pos], mu[contact]);
    }

    // Armijo line search

    int stopingcriteria = 1;
    int i = -1;
    double alpha ;
    double lhs = NAN;
    double rhs;
    // z_k-y_k
    cblas_dcopy(n , reaction , 1 , reactiontmp3, 1);
    cblas_daxpy(n, -1.0, reactiontmp, 1, reactiontmp3, 1);


    while (stopingcriteria && (i < lsitermax))
    {
      i++ ;
      cblas_dcopy(n , reactiontmp , 1 , reactiontmp2, 1);
      alpha = 1.0 / (pow(2.0, i));
#ifdef VERBOSE_DEBUG
      printf("alpha = %f\n", alpha);
#endif
      cblas_dscal(n , alpha, reactiontmp2, 1);
      alpha  = 1.0 - alpha;

      cblas_daxpy(n, alpha, reaction, 1, reactiontmp2, 1);

      cblas_dcopy(n , q , 1 , velocitytmp, 1);

      NM_gemv(1.0, M, reactiontmp2, 1.0, velocitytmp);



      /* #ifdef VERBOSE_DEBUG */
      /*     for (contact = 0 ; contact < nc ; ++contact) */
      /*     { */
      /*       for(int kk=0; kk<3;kk++) printf("reactiontmp2[%i]=%12.8e\t",contact*nLocal+kk,  reactiontmp2[contact*nLocal+kk]); */
      /*       printf("\n"); */
      /*     } */
      /* #endif   */
      lhs = cblas_ddot(n, velocitytmp, 1, reactiontmp3, 1);
      rhs = cblas_dnrm2(n, reactiontmp3, 1);
      rhs = sigma / rho * rhs * rhs;
      if (lhs >= rhs)  stopingcriteria = 0;
#ifdef VERBOSE_DEBUG
      printf("Number of iteration in Armijo line search = %i\n", i);
      printf("lhs = %f\n", lhs);
      printf("rhs = %f\n", rhs);
      printf("alpha = %f\n", alpha);
      printf("sigma = %f\n", sigma);
      printf("rho = %f\n", rho);
#endif
    }

    double nonorm = cblas_dnrm2(n, velocitytmp, 1);
    double rhoequiv = lhs / (nonorm * nonorm);
#ifdef VERBOSE_DEBUG
    printf("rho equiv = %f\n", rhoequiv);
#endif
    cblas_daxpy(n, -rhoequiv, velocitytmp, 1, reaction  , 1);


    // projection for each contact
    for (contact = 0 ; contact < nc ; ++contact)
    {
      int pos = contact * nLocal;
      projectionOnCone(&reaction[pos], mu[contact]);
    }

    /* **** 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 (verbose > 0)
      printf("--------------- FC3D - Hyperplane Projection (HP) - Iteration %i rho = %14.7e \t rhoequiv = %14.7e \tError = %14.7e\n", iter, rho, rhoequiv, error);

    if (error < tolerance) hasNotConverged = 0;
    *info = hasNotConverged;
  }
  if (verbose > 0)
    printf("--------------- FC3D - Hyperplane Projection (HP) - #Iteration %i Final Residual = %14.7e\n", iter, error);
  dparam[0] = tolerance;
  dparam[1] = error;
  iparam[7] = iter;
  free(velocitytmp);
  free(reactiontmp);
  free(reactiontmp2);
  free(reactiontmp3);

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

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

}
int frictionContactFBLSA(
  fc3d_nonsmooth_Newton_solvers* equation,
  double *reaction,
  double *velocity,
  double *mu,
  double *rho,
  double *F,
  double *A,
  double *B,
  NumericsMatrix *W,
  double *qfree,
  NumericsMatrix *blockAWpB,
  double *direction,
  double *tmp,
  double alpha[1],
  unsigned int maxiter_ls)
{
  unsigned problemSize = W->size0;

  // notes :
  // - F contains FB or grad FB merit
  // - tmp contains direction, scal*direction, reaction+scal*direction

  // cf Newton_Methods.c, L59
  double p = 2.1;
  double fblsa_rho = 1e-8;
  double gamma = 1e-4;
  double scal = 1.;

  // F <- compute fb
  fc3d_FischerBurmeisterFunction(problemSize,
                                              (FischerBurmeisterFun3x3Ptr) fc3d_FischerBurmeisterFunctionGenerated,
                                              reaction,
                                              velocity,
                                              mu,
                                              rho,
                                              F,
                                              NULL,
                                              NULL);

  double thetafb0 = 0.5 * cblas_ddot(problemSize, F, 1, F, 1);

  // F <- compute gradient of fb merit function (ugly)
  fc3d_FischerBurmeisterFunction(problemSize,
                                              (FischerBurmeisterFun3x3Ptr) fc3d_FischerBurmeisterGradMeritFunctionGenerated,
                                              reaction,
                                              velocity,
                                              mu,
                                              rho,
                                              F,
                                              NULL,
                                              NULL);
  double norm_dir_exp_p = pow(cblas_dnrm2(problemSize, direction, 1), p);
  double gradmeritfb_dir = cblas_ddot(problemSize, F, 1, direction, 1);

  if (!isnan(gradmeritfb_dir) && !isinf(gradmeritfb_dir) && gradmeritfb_dir > (-fblsa_rho * norm_dir_exp_p))
  {
    if (verbose > 0)
    {
      printf("fc3d FBLSA: condition 9.1.6 unsatisfied, gradmeritfb_dir=%g, norm_r=%g\n", gradmeritfb_dir, norm_dir_exp_p);
    }

    // FIX: failure...
    if (verbose > 0)
    {
      printf("fc3d FBLSA: set d^k to - grad merit(fb)\n");
    }

    cblas_dcopy(problemSize, F, 1, direction, 1);
    cblas_dscal(problemSize, -1, direction, 1);
  }

  for (unsigned int iter = 0; iter < maxiter_ls; ++iter)
  {

    scal /= 2.;

    // tmp <- 2^(-ik)*direction+reaction
    cblas_dcopy(problemSize, reaction, 1, tmp, 1);
    cblas_daxpy(problemSize, scal, direction, 1, tmp, 1);

    // velocity <- W*tmp + qfree
    cblas_dcopy(problemSize, qfree, 1, velocity, 1);
    NM_gemv(1., W, tmp, 1., velocity);

    // compute fb
    fc3d_FischerBurmeisterFunction(problemSize,
                                                (FischerBurmeisterFun3x3Ptr) fc3d_FischerBurmeisterFunctionGenerated,
                                                tmp,
                                                velocity,
                                                mu,
                                                rho,
                                                F,
                                                NULL,
                                                NULL);

    double thetafb  = 0.5 * cblas_ddot(problemSize, F, 1, F, 1);

    // compute grad merit fb (ugly)
    fc3d_FischerBurmeisterFunction(problemSize,
                                                (FischerBurmeisterFun3x3Ptr) fc3d_FischerBurmeisterGradMeritFunctionGenerated,
                                                tmp,
                                                velocity,
                                                mu,
                                                rho,
                                                F,
                                                NULL,
                                                NULL);

    // tmp <- scal*direction
    cblas_dscal(problemSize, 0., tmp, 1);
    cblas_daxpy(problemSize, scal, direction, 1, tmp, 1);
    double grad_meritf_reaction = cblas_ddot(problemSize, F, 1, tmp, 1);

    if (!isinf(grad_meritf_reaction) && !isnan(grad_meritf_reaction) &&
        thetafb < thetafb0 + gamma * scal * grad_meritf_reaction)
    {
      if (verbose > 0)
      {
        printf("fc3d FBLSA success. iteration  = %i, thetafb=%g, thetafb0=%g, gradmeritf,reaction=%g\n", iter, thetafb, thetafb0, gamma*scal*grad_meritf_reaction);
      }
      // tmp <- reaction + tmp
      cblas_daxpy(problemSize, 1, reaction, 1, tmp, 1);

      return 0;
    }
  }

  if (verbose > 0)
  {
    printf("fc3d FBLSA reached the max number of iteration reached  = %i\n", maxiter_ls);
  }

  return -1;
}
int globalLineSearchGP(
  fc3d_nonsmooth_Newton_solvers* equation,
  double *reaction,
  double *velocity,
  double *mu,
  double *rho,
  double *F,
  double *A,
  double *B,
  NumericsMatrix *W,
  double *qfree,
  NumericsMatrix *AWpB,
  double *direction,
  double *tmp,
  double alpha[1],
  unsigned int maxiter_ls)
{
  unsigned problemSize = W->size0;

  double inf = 1e10;
  double alphamin = 0.0;
  double alphamax = inf;

  double m1 = 0.01, m2 = 0.99;

  // Computation of q(t) and q'(t) for t =0

  double q0 = 0.5 * cblas_ddot(problemSize, F, 1, F, 1);

  if (isnan(q0) || isinf(q0))
  {
    if (verbose > 0)
    {
      fprintf(stderr, "global line search warning. q0 is not a finite number.\n");
    }
    return -1;
  }

  //  tmp <- AWpB * direction
  NM_gemv(1., AWpB, direction, 0., tmp);

  double dqdt0 = cblas_ddot(problemSize, F, 1, tmp, 1);

  for (unsigned int iter = 0; iter < maxiter_ls; ++iter)
  {

    // tmp <- alpha*direction+reaction
    cblas_dcopy(problemSize, reaction, 1, tmp, 1);
    cblas_daxpy(problemSize, alpha[0], direction, 1, tmp, 1);

    // velocity <- W*tmp + qfree
    cblas_dcopy(problemSize, qfree, 1, velocity, 1);
    NM_gemv(1., W, tmp, 1., velocity);

    equation->function(equation->data, problemSize, tmp,
                       velocity, mu, rho, F,
                       NULL, NULL);

    double q  = 0.5 * cblas_ddot(problemSize, F, 1, F, 1);

    if (isnan(q) || isinf(q))
    {
      printf("global line search warning. q is not a finite number.\n");
      return -1;
    }

    assert(q >= 0);

    double slope = (q - q0) / alpha[0];

    int C1 = (slope >= m2 * dqdt0);
    int C2 = (slope <= m1 * dqdt0);

    if (C1 && C2)
    {
      if (verbose > 0)
      {
        printf("             globalLineSearchGP. success. ls_iter = %i  alpha = %.10e, q = %.10e\n", iter, alpha[0], q);
      }

      return 0;

    }
    else if (!C1)
    {
      alphamin = alpha[0];
    }
    else
    {
      // not(C2)
      alphamax = alpha[0];
    }

    if (alpha[0] < inf)
    {
      alpha[0] = 0.5 * (alphamin + alphamax);
    }
    else
    {
      alpha[0] = alphamin;
    }

  }
  if (verbose > 0)
  {
    printf("global line search reached the  max number of iteration  = %i  with alpha = %.10e \n", maxiter_ls, alpha[0]);
  }

  return -1;
}
Exemplo n.º 11
0
int soclcp_compute_error(
  SecondOrderConeLinearComplementarityProblem* problem,
  double *z , double *w, double tolerance,
  SolverOptions * options, double * error)
{
  assert(problem);
  assert(z);
  assert(w);
  assert(error);

  /* Computes w = Mz + q */
  int incx = 1, incy = 1;
  int nc = problem->nc;
  double *mu = problem->tau;
  int n = problem->n;
  
  cblas_dcopy(n , problem->q , incx , w , incy); // w <-q
  // Compute the current velocity
  NM_gemv(1.0, problem->M, z, 1.0, w);

  /* for (int i=0; i < n ; i++ ) */
  /* { */
  /*   printf("w[%i]=%e\t\t\t",i,w[i]); */
  /*   printf("z[%i]=%e\n",i,z[i]); */
  /* } */
  /* printf("\n"); */
  
  *error = 0.;

  int ic;
  int dim;
  unsigned int dim_max=0;
  for (int i =0; i <nc; i++)
  {
    dim_max=max(dim_max,problem->coneIndex[i+1]-problem->coneIndex[i]);
  }
  double *worktmp = (double *)calloc(dim_max,sizeof(double));

  for(ic = 0 ; ic < nc ; ic++)
  {
    dim = problem->coneIndex[ic+1]- problem->coneIndex[ic];
    soclcp_unitary_compute_and_add_error(z + problem->coneIndex[ic],
                                         w + problem->coneIndex[ic],
                                         dim , mu[ic], error, worktmp);
    /* for (int i=0; i < dim; i++ ) */
    /* { */
    /*   printf("-- w[%i]=%e\t\t\t",i,(w + problem->coneIndex[ic])[i]); */
    /*   printf("z[%i]=%e\n",i,(z + problem->coneIndex[ic])[i]); */
    /* } */
  }
  free(worktmp);
  *error = sqrt(*error);

  /* Computes error */
  double norm_q = cblas_dnrm2(n , problem->q , incx);
  DEBUG_PRINTF("norm_q = %12.8e\n", norm_q);
  *error = *error / (norm_q + 1.0);
  
  if(*error > tolerance)
  {
    if(verbose > 1)
      printf(" Numerics - soclcp_compute_error: error = %g > tolerance = %g.\n",
             *error, tolerance);
    return 1;
  }
  else
    return 0;
}
/* Alart & Curnier solver for sparse global problem */
void gfc3d_nonsmooth_Newton_AlartCurnier(
  GlobalFrictionContactProblem* problem,
  double *reaction,
  double *velocity,
  double *globalVelocity,
  int *info,
  SolverOptions* options)
{

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

  assert(problem->dimension == 3);

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

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

  assert(!problem->M->matrix0);
//  assert(problem->M->matrix1);

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

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

  assert(problem->M->size0 == problem->H->size0);

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

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

  assert(itermax > 0);
  assert(options->iparam[3] > 0);

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

  if (verbose > 0)
    printf("------------------------ GFC3D - _nonsmooth_Newton_AlartCurnier - Start with tolerance = %g\n", tolerance);


  /* sparse triplet storage */
  NM_triplet(problem->M);
  NM_triplet(problem->H);

  unsigned int ACProblemSize = sizeOfPsi(NM_triplet(problem->M),
                                         NM_triplet(problem->H));

  unsigned int globalProblemSize = (unsigned)NM_triplet(problem->M)->m;

  unsigned int localProblemSize = problem->H->size1;

  assert((int)localProblemSize == problem->numberOfContacts * problem->dimension);

  assert((int)globalProblemSize == problem->H->size0); /* size(velocity) ==
                                                   * Htrans*globalVelocity */


  AlartCurnierFun3x3Ptr computeACFun3x3 = NULL;

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

  if(options->iparam[9] == 0)
  {
    /* allocate memory */
    assert(options->dWork == NULL);
    assert(options->iWork == NULL);
    options->dWork = (double *) malloc(
                       (localProblemSize + /* F */
                        3 * localProblemSize + /* A */
                        3 * localProblemSize + /* B */
                        localProblemSize + /* rho */
                        ACProblemSize + /* psi */
                        ACProblemSize + /* rhs */
                        ACProblemSize + /* tmp2 */
                        ACProblemSize + /* tmp3 */
                        ACProblemSize   /* solution */) *  sizeof(double));

    /* XXX big hack here */
    options->iWork = (int *) malloc(
                       (3 * localProblemSize + /* iA */
                        3 * localProblemSize + /* iB */
                        3 * localProblemSize + /* pA */
                        3 * localProblemSize)  /* pB */
                       * sizeof(csi));

    options->iparam[9] = 1;

  }

  assert(options->dWork != NULL);
  assert(options->iWork != NULL);

  double *F = options->dWork;
  double *A = F +   localProblemSize;
  double *B = A +   3 * localProblemSize;
  double *rho = B + 3 * localProblemSize;

  double * psi = rho + localProblemSize;
  double * rhs = psi + ACProblemSize;
  double * tmp2 = rhs + ACProblemSize;
  double * tmp3 = tmp2 + ACProblemSize;
  double * solution = tmp3 + ACProblemSize;

  /* XXX big hack --xhub*/
  csi * iA = (csi *)options->iWork;
  csi * iB = iA + 3 * localProblemSize;
  csi * pA = iB + 3 * localProblemSize;
  csi * pB = pA + 3 * localProblemSize;

  CSparseMatrix A_;
  CSparseMatrix B_;
  CSparseMatrix *J;

  A_.p = pA;
  B_.p = pB;
  A_.i = iA;
  B_.i = iB;

  init3x3DiagBlocks(problem->numberOfContacts, A, &A_);
  init3x3DiagBlocks(problem->numberOfContacts, B, &B_);

  J = cs_spalloc(NM_triplet(problem->M)->n + A_.m + B_.m,
                 NM_triplet(problem->M)->n + A_.m + B_.m,
                 NM_triplet(problem->M)->nzmax + 2*NM_triplet(problem->H)->nzmax +
                 2*A_.n + A_.nzmax + B_.nzmax, 1, 1);

  assert(A_.n == problem->H->size1);
  assert(A_.nz == problem->numberOfContacts * 9);
  assert(B_.n == problem->H->size1);
  assert(B_.nz == problem->numberOfContacts * 9);

  fc3d_AlartCurnierFunction(
    localProblemSize,
    computeACFun3x3,
    reaction, velocity,
    problem->mu, rho,
    F, A, B);

  csi Astart = initACPsiJacobian(NM_triplet(problem->M),
                                 NM_triplet(problem->H),
                                 &A_, &B_, J);

  assert(Astart > 0);

  assert(A_.m == A_.n);
  assert(B_.m == B_.n);

  assert(A_.m == problem->H->size1);

  // compute rho here
  for(unsigned int i = 0; i < localProblemSize; ++i) rho[i] = 1.;

  // direction
  for(unsigned int i = 0; i < ACProblemSize; ++i) rhs[i] = 0.;



  // quick hack to make things work
  // need to use the functions from NumericsMatrix --xhub


  NumericsMatrix *AA_work = createNumericsMatrix(NM_SPARSE,  (int)J->m, (int)J->n);

  NumericsSparseMatrix* SM = newNumericsSparseMatrix();
  SM->triplet = J;
  NumericsMatrix *AA = createNumericsMatrixFromData(NM_SPARSE,  (int)J->m, (int)J->n, SM);

  info[0] = 1;

  /* update local velocity from global velocity */
  /* an assertion ? */
  cblas_dcopy(localProblemSize, problem->b, 1, velocity, 1);
  NM_tgemv(1., problem->H, globalVelocity, 1, velocity);
  double linear_solver_residual=0.0;
  while(iter++ < itermax)
  {

    /* compute psi */
    ACPsi(problem, computeACFun3x3, globalVelocity, reaction, velocity, rho, psi);

    /* compute A & B */
    fc3d_AlartCurnierFunction(localProblemSize,
                              computeACFun3x3,
                              reaction, velocity,
                              problem->mu, rho,
                              F, A, B);
    /* update J */
    updateACPsiJacobian(NM_triplet(problem->M),
                        NM_triplet(problem->H),
                        &A_, &B_, J, Astart);

    /* rhs = -psi */
    cblas_dcopy(ACProblemSize, psi, 1, rhs, 1);
    cblas_dscal(ACProblemSize, -1., rhs, 1);

    /* get compress column storage for linear ops */
    CSparseMatrix* Jcsc = cs_compress(J);

    /* Solve: J X = -psi */

    /* Solve: AWpB X = -F */
    NM_copy(AA, AA_work);

    int info_solver = NM_gesv(AA_work, rhs);
    if (info_solver > 0)
    {
      fprintf(stderr, "------------------------ GFC3D - NSN_AC - solver failed info = %d\n", info_solver);
      break;
      info[0] = 2;
      CHECK_RETURN(!cs_check_triplet(NM_triplet(AA_work)));
    }

    /* Check the quality of the solution */
    if (verbose > 0)
    {
      cblas_dcopy_msan(ACProblemSize, psi, 1, tmp3, 1);
      NM_gemv(1., AA, rhs, 1., tmp3);
      linear_solver_residual = cblas_dnrm2(ACProblemSize, 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;

    /* set current solution */
    for(unsigned int i = 0; i < globalProblemSize; ++i)
    {
      solution[i] = globalVelocity[i];
    }
    for(unsigned int i = 0; i < localProblemSize; ++i)
    {
      solution[i+globalProblemSize] = velocity[i];
      solution[i+globalProblemSize+localProblemSize] = reaction[i];
    }

    DEBUG_EXPR_WE(
      for(unsigned int i = 0; i < globalProblemSize; ++i)
      {
        printf("globalVelocity[%i] = %6.4e\n",i,globalVelocity[i]);
      }
      for(unsigned int i = 0; i < localProblemSize; ++i)
      {
        printf("velocity[%i] = %6.4e\t",i,velocity[i]);
        printf("reaction[%i] = %6.4e\n",i,reaction[i]);
      }
      );


    int info_ls = _globalLineSearchSparseGP(problem,
                                            computeACFun3x3,
                                            solution,
                                            rhs,
                                            globalVelocity,
                                            reaction, velocity,
                                            problem->mu, rho, F, psi, Jcsc,
                                            tmp2, &alpha, 100);


    cs_spfree(Jcsc);
    if(!info_ls)
    {
      cblas_daxpy(ACProblemSize, alpha, rhs, 1, solution, 1);
    }
    else
    {
      cblas_daxpy(ACProblemSize, 1, rhs, 1., solution, 1);
    }

    for(unsigned int e = 0 ; e < globalProblemSize; ++e)
    {
      globalVelocity[e] = solution[e];
    }

    for(unsigned int e = 0 ; e < localProblemSize; ++e)
    {
      velocity[e] = solution[e+globalProblemSize];
    }

    for(unsigned int e = 0; e < localProblemSize; ++e)
    {
      reaction[e] = solution[e+globalProblemSize+localProblemSize];
    }

    options->dparam[1] = INFINITY;

    if(!(iter % erritermax))
    {

      gfc3d_compute_error(problem,
                          reaction, velocity, globalVelocity,
                          tolerance,
                          &(options->dparam[1]));
    }

    if(verbose > 0)
      printf("------------------------ GFC3D - NSN_AC - iteration %d, residual = %g, linear solver residual = %g, tolerance = %g \n", iter, options->dparam[1],linear_solver_residual, tolerance);

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


  }