void fc3d_ProjectedGradientOnCylinder(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;
/* Dimension of the problem */
int n = 3 * nc;
/* Maximum number of iterations */
int itermax = iparam[0];
/* Tolerance */
double tolerance = dparam[0];
/***** Projected Gradient iterations *****/
int j, 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 *)malloc(n * sizeof(double));
double rho = 0.0;
int isVariable = 0;
double rhoinit, rhomin;
if (dparam[3] > 0.0)
{
rho = dparam[3];
}
else
{
/* Variable step in fixed*/
isVariable = 1;
printf("Variable step (line search) in Projected Gradient iterations\n");
rhoinit = dparam[3];
rhomin = dparam[4];
}
double * reactionold;
double * direction;
if (isVariable)
{
reactionold = (double *)malloc(n * sizeof(double));
direction = (double *)malloc(n * sizeof(double));
}
double alpha = 1.0;
double beta = 1.0;
/* double minusrho = -1.0*rho; */
if (!isVariable)
{
while ((iter < itermax) && (hasNotConverged > 0))
{
++iter;
cblas_dcopy(n , q , 1 , velocitytmp, 1);
prodNumericsMatrix(n, n, alpha, M, reaction, beta, velocitytmp);
// projection for each contact
cblas_daxpy(n, -1.0, velocitytmp, 1, reaction , 1);
for (contact = 0 ; contact < nc ; ++contact)
projectionOnCylinder(&reaction[ contact * nLocal],
options->dWork[contact]);
#ifdef VERBOSE_DEBUG
printf("reaction before LS\n");
for (contact = 0 ; contact < nc ; ++contact)
{
for (j = 0; j < 3; j++)
printf("reaction[%i] = %le\t", 3 * contact + j, reaction[3 * contact + j]);
printf("\n");
}
printf("velocitytmp before LS\n");
for (contact = 0 ; contact < nc ; ++contact)
{
for (j = 0; j < 3; j++)
printf("velocitytmp[%i] = %le\t", 3 * contact + j, velocitytmp[3 * contact + j]);
printf("\n");
}
#endif
/* **** Criterium convergence **** */
fc3d_Tresca_compute_error(problem, reaction , velocity, tolerance, options, &error);
if (options->callback)
{
options->callback->collectStatsIteration(options->callback->env, nc * 3,
reaction, velocity,
error, NULL);
}
if (verbose > 0)
printf("----------------------------------- FC3D - Projected Gradient On Cylinder (PGoC) - Iteration %i rho = %14.7e \tError = %14.7e\n", iter, rho, error);
if (error < tolerance) hasNotConverged = 0;
*info = hasNotConverged;
}
}
else
{
rho = rhoinit;
cblas_dcopy(n , q , 1 , velocitytmp, 1);
prodNumericsMatrix(n, n, 1.0, M, reaction, 1.0, velocitytmp);
cblas_daxpy(n, rho, velocitytmp, 1, reaction, 1);
for (contact = 0 ; contact < nc ; ++contact)
projectionOnCylinder(&reaction[contact * nLocal],
options->dWork[contact]);
cblas_dcopy(n , q , 1 , velocitytmp, 1);
prodNumericsMatrix(n, n, 1.0, M, reaction, 1.0, velocitytmp);
double oldcriterion = cblas_ddot(n, reaction, 1, velocitytmp, 1);
#ifdef VERBOSE_DEBUG
printf("oldcriterion =%le \n", oldcriterion);
#endif
while ((iter < itermax) && (hasNotConverged > 0))
{
++iter;
// store the old reaction
cblas_dcopy(n , reaction , 1 , reactionold , 1);
// compute the direction
cblas_dcopy(n , q , 1 , velocitytmp, 1);
prodNumericsMatrix(n, n, 1.0, M, reaction, 1.0, velocitytmp);
cblas_dcopy(n, velocitytmp, 1, direction, 1);
// start line search
j = 0;
if (rho <= 100 * rhoinit) rho = 10.0 * rho;
double newcriterion = 1e24;
do
{
cblas_dcopy(n , reactionold , 1 , reaction , 1);
cblas_daxpy(n, rho, direction, 1, reaction , 1) ;
#ifdef VERBOSE_DEBUG
printf("LS iteration %i step 0 \n", j);
printf("rho = %le \n", rho);
for (contact = 0 ; contact < nc ; ++contact)
{
for (int k = 0; k < 3; k++)
printf("reaction[%i] = %le\t",
3 * contact + k, reaction[3 * contact + k]);
printf("\n");
}
#endif
for (contact = 0 ; contact < nc ; ++contact)
projectionOnCylinder(&reaction[contact * nLocal],
options->dWork[contact]);
/* printf("options->dWork[%i] = %le\n",contact, options->dWork[contact] );} */
#ifdef VERBOSE_DEBUG
printf("LS iteration %i step 1 after projection\n", j);
for (contact = 0 ; contact < nc ; ++contact)
{
for (int k = 0; k < 3; k++)
printf("reaction[%i] = %le\t",
3 * contact + k, reaction[3 * contact + k]);
printf("\n");
}
#endif
cblas_dcopy(n , q , 1 , velocitytmp, 1);
prodNumericsMatrix(n, n, 1.0, M, reaction, 1.0, velocitytmp);
#ifdef VERBOSE_DEBUG
printf("LS iteration %i step 3 \n", j);
for (contact = 0 ; contact < nc ; ++contact)
{
for (int k = 0; k < 3; k++)
printf("velocitytmp[%i] = %le\t", 3 * contact + k, velocitytmp[3 * contact + k]);
printf("\n");
}
#endif
newcriterion = cblas_ddot(n, reaction, 1, velocitytmp, 1);
#ifdef VERBOSE_DEBUG
printf("LS iteration %i newcriterion =%le\n", j, newcriterion);
#endif
if (rho > rhomin)
{
rho = rhomin;
break;
}
rho = 0.5 * rho;
}
while (newcriterion > oldcriterion &&
++j <= options->iparam[2]);
oldcriterion = newcriterion;
/* **** Criterium convergence **** */
fc3d_Tresca_compute_error(problem, reaction , velocity, tolerance, options, &error);
if (verbose > 0)
printf("----------------------------------- FC3D - Projected Gradient On Cylinder (PGoC) - Iteration %i rho = %14.7e \tError = %14.7e\n", iter, rho, error);
if (error < tolerance) hasNotConverged = 0;
*info = hasNotConverged;
}
}
printf("----------------------------------- FC3D - Projected Gradient On Cylinder (PGoC)- #Iteration %i Final Residual = %14.7e\n", iter, error);
dparam[0] = tolerance;
dparam[1] = error;
free(velocitytmp);
if (isVariable)
{
free(reactionold);
free(direction);
}
}
void fc3d_ConvexQP_ProjectedGradient_Cylinder(FrictionContactProblem* problem, double *reaction, double *velocity, int* info, SolverOptions* options)
{
/* Number of contacts */
int nc = problem->numberOfContacts;
/* Dimension of the problem */
int n = 3 * nc;
ConvexQP *cqp = (ConvexQP *)malloc(sizeof(ConvexQP));
cqp->size=n;
cqp->M = problem->M;
cqp->q = problem->q;
cqp->A = NULL; /* The A matrix is the identity and b is equal to zero */
cqp->b = NULL;
cqp->ProjectionOnC = &Projection_ConvexQP_FC3D_Cylinder;
int iter=0;
double error=1e24;
FrictionContactProblem_as_ConvexQP *fc3d_as_cqp= (FrictionContactProblem_as_ConvexQP*)malloc(sizeof(FrictionContactProblem_as_ConvexQP));
cqp->env = fc3d_as_cqp ;
cqp->size = n;
/*set the norm of the ConvexQP to the norm of problem->q */
double norm_q = cblas_dnrm2(nc*3 , problem->q , 1);
cqp->normConvexQP= norm_q;
cqp->istheNormConvexQPset=1;
fc3d_as_cqp->cqp = cqp;
fc3d_as_cqp->fc3d = problem;
fc3d_as_cqp->options = options;
/* frictionContact_display(fc3d_as_cqp->fc3d); */
SolverOptions * cqpsolver_options = (SolverOptions *) malloc(sizeof(SolverOptions));
convexQP_ProjectedGradient_setDefaultSolverOptions(cqpsolver_options);
int isize = options->iSize;
int dsize = options->dSize;
int cqp_isize = cqpsolver_options->iSize;
int cqp_dsize = cqpsolver_options->dSize;
if (isize != cqp_isize )
{
printf("Warning: options->iSize in fc3d_ConvexQP_FixedPointProjection is not consitent with options->iSize in ConvexQP_FPP\n");
}
if (dsize != cqp_dsize )
{
printf("Warning: options->iSize in fc3d_ConvexQP_FixedPointProjection is not consitent with options->iSize in ConvexQP_FPP\n");
}
int i;
for (i = 0; i < min(isize,cqp_isize); i++)
{
if (options->iparam[i] != 0 )
cqpsolver_options->iparam[i] = options->iparam[i] ;
}
for (i = 0; i < min(dsize,cqp_dsize); i++)
{
if (fabs(options->dparam[i]) >= 1e-24 )
cqpsolver_options->dparam[i] = options->dparam[i] ;
}
convexQP_ProjectedGradient(cqp, reaction, velocity , info , cqpsolver_options);
/* **** Criterium convergence **** */
fc3d_Tresca_compute_error(problem, reaction , velocity, 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 = cqpsolver_options->dparam[1];
iter = cqpsolver_options->iparam[7];
options->dparam[SICONOS_DPARAM_RESIDU] = error;
options->iparam[SICONOS_IPARAM_ITER_DONE] = iter;
if (verbose > 0)
{
printf("--------------- FC3D - ConvexQP Fixed Point Projection (ConvexQP_FPP) - #Iteration %i Final Residual = %14.7e\n", iter, error);
}
free(cqp);
solver_options_delete(cqpsolver_options);
free(cqpsolver_options);
free(fc3d_as_cqp);
}
