int frictionContact3D_projectionOnCylinder_solve(FrictionContactProblem *localproblem , double* reaction, SolverOptions* options)
{
/* int and double parameters */
/* int* iparam = options->iparam; */
/* double* dparam = options->dparam; */
double * MLocal = localproblem->M->matrix0;
double * qLocal = localproblem->q;
/* int nLocal = 3; */
/* Builds local problem for the current contact */
/* frictionContact3D_projection_update(contact, reaction); */
/*double an = 1./(MLocal[0]);*/
/* double alpha = MLocal[nLocal+1] + MLocal[2*nLocal+2]; */
/* double det = MLocal[1*nLocal+1]*MLocal[2*nLocal+2] - MLocal[2*nLocal+1] + MLocal[1*nLocal+2]; */
/* double beta = alpha*alpha - 4*det; */
/* double at = 2*(alpha - beta)/((alpha + beta)*(alpha + beta)); */
double an = 1. / (MLocal[0]);
int i;
/* int incx = 1, incy = 1; */
double worktmp[3];
double R = localproblem->mu[options->iparam[4]];
//printf("R=%e\n", R);
/* cblas_dcopy(nLocal , qLocal, incx , worktmp , incy); */
/* cblas_dgemv(CblasColMajor,CblasNoTrans, nLocal, nLocal, 1.0, MLocal, 3, reaction, incx, 1.0, worktmp, incy); */
for (i = 0; i < 3; i++) worktmp[i] = MLocal[i + 0 * 3] * reaction[0] + qLocal[i]
+ MLocal[i + 1 * 3] * reaction[1] +
+ MLocal[i + 2 * 3] * reaction[2] ;
reaction[0] -= an * worktmp[0];
reaction[1] -= an * worktmp[1];
reaction[2] -= an * worktmp[2];
projectionOnCylinder(reaction, R);
return 0;
}
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);
}
}
int frictionContact3D_projectionOnCylinderWithLocalIteration_solve(FrictionContactProblem* localproblem, double* reaction, SolverOptions* options)
{
/* int and double parameters */
int* iparam = options->iparam;
double* dparam = options->dparam;
double * MLocal = localproblem->M->matrix0;
double * qLocal = localproblem->q;
/* int nLocal = 3; */
/* /\* Builds local problem for the current contact *\/ */
/* frictionContact3D_projection_update(localproblem, reaction); */
/*double an = 1./(MLocal[0]);*/
/* double alpha = MLocal[nLocal+1] + MLocal[2*nLocal+2]; */
/* double det = MLocal[1*nLocal+1]*MLocal[2*nLocal+2] - MLocal[2*nLocal+1] + MLocal[1*nLocal+2]; */
/* double beta = alpha*alpha - 4*det; */
/* double at = 2*(alpha - beta)/((alpha + beta)*(alpha + beta)); */
/* double an = 1. / (MLocal[0]); */
/* double at = 1.0 / (MLocal[4] + mu_i); */
/* double as = 1.0 / (MLocal[8] + mu_i); */
/* at = an; */
/* as = an; */
double rho= options->dWork[options->iparam[4]] , rho_k;
/* printf ("saved rho = %14.7e\n",rho ); */
/* printf ("options->iparam[4] = %i\n",options->iparam[4] ); */
/* int incx = 1, incy = 1; */
int i;
double velocity[3],velocity_k[3],reaction_k[3];
double localerror = 1.0;
//printf ("localerror = %14.7e\n",localerror );
int localiter = 0;
double localtolerance = dparam[0];
/* Variable for Line_search */
double a1,a2;
int success = 0;
double localerror_k;
int ls_iter = 0;
int ls_itermax = 10;
/* double tau=dparam[4], tauinv=dparam[5], L= dparam[6], Lmin = dparam[7]; would be better */
double tau=2.0/3.0, tauinv = 3.0/2.0, L= 0.9, Lmin =0.3;
double R = localproblem->mu[options->iparam[4]];
/* printf ("R = %14.7e\n",R ); */
while ((localerror > localtolerance) && (localiter < iparam[0]))
{
localiter ++;
/* printf ("reaction[0] = %14.7e\n",reaction[0]); */
/* printf ("reaction[1] = %14.7e\n",reaction[1]); */
/* printf ("reaction[2] = %14.7e\n",reaction[2]); */
/* Store the error */
localerror_k = localerror;
/* /\* store the reaction at the beginning of the iteration *\/ */
/* cblas_dcopy_msan(nLocal , reaction , 1 , reaction_k, 1); */
/* /\* velocity_k <- q *\/ */
/* cblas_dcopy_msan(nLocal , qLocal , 1 , velocity_k, 1); */
/* /\* velocity_k <- q + M * reaction *\/ */
/* cblas_dgemv(CblasColMajor,CblasNoTrans, nLocal, nLocal, 1.0, MLocal, 3, reaction, incx, 1.0, velocity_k, incy); */
reaction_k[0]=reaction[0];
reaction_k[1]=reaction[1];
reaction_k[2]=reaction[2];
/* /\* velocity_k <- q *\/ */
/* cblas_dcopy_msan(nLocal , qLocal , 1 , velocity_k, 1); */
/* /\* velocity_k <- q + M * reaction *\/ */
/* cblas_dgemv(CblasColMajor,CblasNoTrans, nLocal, nLocal, 1.0, MLocal, 3, reaction, incx, 1.0, velocity_k, incy); */
for (i = 0; i < 3; i++) velocity_k[i] = MLocal[i + 0 * 3] * reaction[0] + qLocal[i]
+ MLocal[i + 1 * 3] * reaction[1] +
+ MLocal[i + 2 * 3] * reaction[2] ;
ls_iter = 0 ;
success =0;
rho_k=rho / tau;
while (!success && (ls_iter < ls_itermax))
{
rho_k = rho_k * tau ;
reaction[0] = reaction_k[0] - rho_k * velocity_k[0];
reaction[1] = reaction_k[1] - rho_k * velocity_k[1];
reaction[2] = reaction_k[2] - rho_k * velocity_k[2];
projectionOnCylinder(&reaction[0], R);
/* /\* velocity <- q *\/ */
/* cblas_dcopy(nLocal , qLocal , 1 , velocity, 1); */
/* /\* velocity <- q + M * reaction *\/ */
/* cblas_dgemv(CblasColMajor,CblasNoTrans, nLocal, nLocal, 1.0, MLocal, 3, reaction, incx, 1.0, velocity, incy); */
for (i = 0; i < 3; i++) velocity[i] = MLocal[i + 0 * 3] * reaction[0] + qLocal[i]
+ MLocal[i + 1 * 3] * reaction[1] +
+ MLocal[i + 2 * 3] * reaction[2] ;
a1 = sqrt((velocity_k[0] - velocity[0]) * (velocity_k[0] - velocity[0]) +
(velocity_k[1] - velocity[1]) * (velocity_k[1] - velocity[1]) +
(velocity_k[2] - velocity[2]) * (velocity_k[2] - velocity[2]));
a2 = sqrt((reaction_k[0] - reaction[0]) * (reaction_k[0] - reaction[0]) +
(reaction_k[1] - reaction[1]) * (reaction_k[1] - reaction[1]) +
(reaction_k[2] - reaction[2]) * (reaction_k[2] - reaction[2]));
success = (rho_k*a1 <= L * a2)?1:0;
/* printf("rho_k = %12.8e\t", rho_k); */
/* printf("a1 = %12.8e\t", a1); */
/* printf("a2 = %12.8e\t", a2); */
/* printf("norm reaction = %12.8e\t",sqrt(( reaction[0]) * (reaction[0]) + */
/* ( reaction[1]) * reaction[1]) + */
/* ( reaction[2]) * ( reaction[2])); */
/* printf("success = %i\n", success); */
ls_iter++;
}
/* if (verbose>2) */
/* printf("---------------------- localiter = %i\t, rho= %.10e\t, error = %.10e \n", localiter, rho, localerror); */
/* compute local error */
localerror =0.0;
FrictionContact3D_Tresca_unitary_compute_and_add_error(reaction , velocity, R, &localerror);
/*Update rho*/
if ((rho_k*a1 < Lmin * a2) && (localerror < localerror_k))
{
rho =rho_k*tauinv;
}
else
rho =rho_k;
if (verbose > 1)
{
printf("---------------------- localiter = %i\t, rho= %.10e\t, error = %.10e \n", localiter, rho, localerror);
}
}
options->dWork[options->iparam[4]] =rho;
if (verbose > 1)
{
printf("---------------------- localiter = %i\t, rho= %.10e\t, error = %.10e \n", localiter, rho, localerror);
}
if (localerror > localtolerance)
return 1;
return 0;
}
