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 */ }