int frictionContact3D_projectionOnConeWithLocalIteration_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;
double mu_i = localproblem->mu[0];
/* 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 normUT;
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]; */
double tau=2.0/3.0, tauinv = 3.0/2.0, L= 0.9, Lmin =0.3;
/* printf ("localtolerance = %14.7e\n",localtolerance ); */
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(nLocal , reaction , 1 , reaction_k, 1); */
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;
normUT = sqrt(velocity_k[1] * velocity_k[1] + velocity_k[2] * velocity_k[2]);
while (!success && (ls_iter < ls_itermax))
{
rho_k = rho_k * tau ;
reaction[0] = reaction_k[0] - rho_k * (velocity_k[0] + mu_i * normUT);
reaction[1] = reaction_k[1] - rho_k * velocity_k[1];
reaction[2] = reaction_k[2] - rho_k * velocity_k[2];
projectionOnCone(&reaction[0], mu_i);
/* 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++;
}
/* printf("---------------------- localiter = %i\t, rho= %.10e\t, error = %.10e \n", localiter, rho, localerror); */
/* compute local error */
localerror =0.0;
FrictionContact3D_unitary_compute_and_add_error(reaction , velocity, mu_i, &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 (localerror > localtolerance)
return 1;
return 0;
}
/* calculate merit function for a local problem */
double fclib_merit_local (struct fclib_local *problem, enum fclib_merit merit, struct fclib_solution *solution)
{
struct fclib_matrix * W = problem->W;
struct fclib_matrix * V = problem->V;
struct fclib_matrix * R = problem->R;
double *mu = problem->mu;
double *q = problem->q;
double *s = problem->s;
int d = problem->spacedim;
if (d !=3 )
{
printf("fclib_merit_local for space dimension = %i not yet implemented\n",d);
return 0;
}
double *v = solution->v;
double *r = solution->r;
double *u = solution->u;
double *l = solution->l;
double error_l, error;
double * tmp;
error=0.0;
error_l=0.0;
int i, ic, ic3;
if (merit == MERIT_1)
{
/* cs M_cs; */
/* fclib_matrix_to_cssparse(W, &M_cs); */
/* cs V_cs; */
/* fclib_matrix_to_cssparse(V, &V_cs); */
/* cs R_cs; */
/* fclib_matrix_to_cssparse(R, &R_cs); */
int n_e =0;
if (R) n_e = R->n;
/* compute V^T {r} + R \lambda + s */
if (n_e >0)
{
cs * VT = cs_transpose((cs *)V, 0) ;
tmp = (double *)malloc(n_e*sizeof(double));
for (i =0; i <n_e; i++) tmp[i] = s[i] ;
cs_gaxpy(VT, r, tmp);
cs_gaxpy((cs *)R, l, tmp);
error_l += dnrm2(tmp,n_e)/(1.0 + dnrm2(s,n_e) );
free(tmp);
}
/* compute \hat u = W {r} + V\lambda + q */
tmp = (double *)malloc(W->n*sizeof(double));
for (i =0; i <W->n; i++) tmp[i] = q[i] ;
cs_gaxpy((cs*)V, l, tmp);
cs_gaxpy((cs*)W, r, tmp);
/* Compute natural map */
int nc = W->n/3;
for (ic = 0, ic3 = 0 ; ic < nc ; ic++, ic3 += 3)
{
FrictionContact3D_unitary_compute_and_add_error(r + ic3, tmp + ic3, mu[ic], &error);
}
free(tmp);
error = sqrt(error)/(1.0 + sqrt(dnrm2(q,W->n)) )+error_l;
/* printf("error_l = %12.8e", error_l); */
/* printf("norm of u = %12.8e\n", dnrm2(u,W->n)); */
/* printf("norm of r = %12.8e\n", dnrm2(r,W->n)); */
/* printf("error = %12.8e\n", error); */
return error;
}
return 0; /* TODO */
}
