void projectionOnSecondOrderCone(double* r, double mu, int size) { if (size ==3) { projectionOnCone(r, mu); } else { fprintf(stderr, "Numerics, projectionOnSecondOrderCone f not yet implementes for size != 3 \n"); exit(EXIT_FAILURE); } }
int frictionContact3D_projectionOnCone_velocity_solve(FrictionContactProblem* localproblem, double* velocity, SolverOptions* options) { /* int and double parameters */ /* int* iparam = options->iparam; */ /* double* dparam = options->dparam; */ /* Current block position */ /* Builds local problem for the current contact */ /* frictionContact3D_projection_update(contact, velocity); */ double * MLocal = localproblem->M->matrix0; double * qLocal = localproblem->q; double mu_i = localproblem->mu[0]; /* int nLocal = 3; */ /*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 normUT; /* cblas_dcopy(nLocal , qLocal, incx , worktmp , incy); */ /* cblas_dgemv(CblasColMajor,CblasNoTrans, nLocal, nLocal, 1.0, MLocal, 3, velocity, incx, 1.0, worktmp, incy); */ for (i = 0; i < 3; i++) worktmp[i] = MLocal[i + 0 * 3] * velocity[0] + qLocal[i] + MLocal[i + 1 * 3] * velocity[1] + + MLocal[i + 2 * 3] * velocity[2] ; normUT = sqrt(velocity[1] * velocity[1] + velocity[2] * velocity[2]); velocity[0] -= - mu_i * normUT + an * (worktmp[0]); velocity[1] -= an * worktmp[1]; velocity[2] -= an * worktmp[2]; double invmui = 1.0 / mu_i; projectionOnCone(velocity, invmui); normUT = sqrt(velocity[1] * velocity[1] + velocity[2] * velocity[2]); velocity[0] -= mu_i * normUT; return 0; }
int frictionContact3D_projectionOnCone_solve(FrictionContactProblem* localproblem, double* reaction, SolverOptions * options) { /* /\* Builds local problem for the current contact *\/ */ /* frictionContact3D_projection_update(contact, reaction); */ double * MLocal = localproblem->M->matrix0; double * qLocal = localproblem->q; double mu_i = localproblem->mu[0]; /* int nLocal = 3; */ /* this part is critical for the success of the projection */ /*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]+mu_i); double an = 1. / (MLocal[0]); /* int incx = 1, incy = 1; */ double worktmp[3]; double normUT; /* cblas_dcopy_msan(nLocal , qLocal, incx , worktmp , incy); */ /* cblas_dgemv(CblasColMajor,CblasNoTrans, nLocal, nLocal, 1.0, MLocal, 3, reaction, incx, 1.0, worktmp, incy); */ for (int 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] ; normUT = sqrt(worktmp[1] * worktmp[1] + worktmp[2] * worktmp[2]); reaction[0] -= an * (worktmp[0] + mu_i * normUT); reaction[1] -= an * worktmp[1]; reaction[2] -= an * worktmp[2]; projectionOnCone(reaction, mu_i); return 0; }
void FrictionContact3D_unitary_compute_and_add_error(double *z , double *w, double mu, double * error) { double normUT; double worktmp[3]; /* Compute the modified local velocity */ normUT = hypot(w[1], w[2]); // i.e sqrt(w[ic3p1]*w[ic3p1]+w[ic3p2]*w[ic3p2]); worktmp[0] = z[0] - (w[0] + mu * normUT); worktmp[1] = z[1] - w[1] ; worktmp[2] = z[2] - w[2] ; projectionOnCone(worktmp, mu); worktmp[0] = z[0] - worktmp[0]; worktmp[1] = z[1] - worktmp[1]; worktmp[2] = z[2] - worktmp[2]; *error += worktmp[0] * worktmp[0] + worktmp[1] * worktmp[1] + worktmp[2] * worktmp[2]; }
void Projection_VI_FC3D(void *viIn, double *x, double *PX) { DEBUG_PRINT("Projection_VI_FC3D(void *viIn, double *x, double *PX)\n") VariationalInequality * vi = (VariationalInequality *) viIn; FrictionContactProblem_as_VI* pb = (FrictionContactProblem_as_VI*)vi->env; FrictionContactProblem * fc3d = pb->fc3d; //frictionContact_display(fc3d); int contact =0; int nLocal = fc3d->dimension; int n = fc3d->numberOfContacts* nLocal; cblas_dcopy(n , x , 1 , PX, 1); for (contact = 0 ; contact < fc3d->numberOfContacts ; ++contact) { projectionOnCone(&PX[ contact * nLocal ], fc3d->mu[contact]); } }
void PXtest(void *viIn, double *x, double *PX) { VariationalInequality * vi = (VariationalInequality *) viIn; Problems* pb = (Problems *)vi->env; FrictionContactProblem * fc3d = pb->fc3d; //frictionContact_display(fc3d); int contact =0; int nc = fc3d->numberOfContacts; int nLocal = fc3d->dimension; int n = nc * nLocal; cblas_dcopy(n , x , 1 , PX, 1); for (contact = 0 ; contact < nc ; ++contact) { int pos = contact * nLocal; projectionOnCone(&PX[pos], fc3d->mu[contact]); } }
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; }
void fc3d_DeSaxceFixedPoint(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]; /* Tolerance */ double tolerance = dparam[0]; /***** 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 *)malloc(n * sizeof(double)); double rho = 0.0; if (dparam[3] > 0.0) { rho = dparam[3]; if (verbose > 0) { printf("----------------------------------- FC3D - DeSaxce Fixed Point (DSFP) - Fixed stepsize with rho = %14.7e \n", rho); } } else { numericsError("fc3d_DeSaxceFixedPoint", "The De Saxce fixed point is implemented with a fixed time--step. Use FixedPointProjection (VI_FPP) method for a variable time--step"); } double alpha = 1.0; double beta = 1.0; while ((iter < itermax) && (hasNotConverged > 0)) { ++iter; /* velocitytmp <- q */ cblas_dcopy(n , q , 1 , velocitytmp, 1); /* velocitytmp <- q + M * reaction */ beta = 1.0; prodNumericsMatrix(n, n, alpha, M, reaction, beta, velocitytmp); /* projection for each contact */ 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]); reaction[pos] -= rho * (velocitytmp[pos] + mu[contact] * normUT); reaction[pos + 1] -= rho * velocitytmp[pos + 1]; reaction[pos + 2] -= rho * velocitytmp[pos + 2]; projectionOnCone(&reaction[pos], mu[contact]); } /* **** Criterium convergence **** */ fc3d_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 - DeSaxce Fixed Point (DSFP) - Iteration %i rho = %14.7e \tError = %14.7e\n", iter, rho, error); if (error < tolerance) hasNotConverged = 0; *info = hasNotConverged; } if (verbose > 0) printf("----------------------------------- FC3D - DeSaxce Fixed point (DSFP) - #Iteration %i Final Residual = %14.7e\n", iter, error); iparam[7] = iter; dparam[0] = tolerance; dparam[1] = error; free(velocitytmp); }
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 globalFrictionContact3D_nsgs(GlobalFrictionContactProblem* problem, double *reaction, double *velocity, double *globalVelocity, int* info, SolverOptions* options) { /* int and double parameters */ int* iparam = options->iparam; double* dparam = options->dparam; /* Number of contacts */ int nc = problem->numberOfContacts; int n = problem->M->size0; int m = 3 * nc; NumericsMatrix* M = problem->M; NumericsMatrix* H = problem->H; double* q = problem->q; double* b = problem->b; double* mu = problem->mu; /* Maximum number of iterations */ int itermax = iparam[0]; /* Tolerance */ double tolerance = dparam[0]; /* Check for trivial case */ *info = checkTrivialCaseGlobal(n, q, velocity, reaction, globalVelocity, options); if (*info == 0) return; SolverGlobalPtr local_solver = NULL; FreeSolverGlobalPtr freeSolver = NULL; ComputeErrorGlobalPtr computeError = NULL; /* Connect local solver */ initializeGlobalLocalSolver(n, &local_solver, &freeSolver, &computeError, M, q, mu, iparam); /***** NSGS 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 */ SparseBlockStructuredMatrix *Htrans = (SparseBlockStructuredMatrix*)malloc(sizeof(SparseBlockStructuredMatrix)); if (H->storageType != M->storageType) { // if(verbose==1) fprintf(stderr, "Numerics, GlobalFrictionContact3D_nsgs. H->storageType != M->storageType :This case is not taken into account.\n"); exit(EXIT_FAILURE); } else if (M->storageType == 1) { inverseDiagSBM(M->matrix1); Global_MisInverse = 1; transposeSBM(H->matrix1, Htrans); } else if (M->storageType == 0) { /* Assume that M is not already LU */ int infoDGETRF = -1; Global_ipiv = (int *)malloc(n * sizeof(int)); assert(!Global_MisLU); DGETRF(n, n, M->matrix0, n, Global_ipiv, &infoDGETRF); Global_MisLU = 1; assert(!infoDGETRF); } else { fprintf(stderr, "Numerics, GlobalFrictionContactProblem_nsgs failed M->storageType not compatible.\n"); exit(EXIT_FAILURE); } dparam[0] = dparam[2]; // set the tolerance for the local solver double* qtmp = (double*)malloc(n * sizeof(double)); for (int i = 0; i < n; i++) qtmp[i] = 0.0; while ((iter < itermax) && (hasNotConverged > 0)) { ++iter; /* Solve the first part with the current reaction */ /* qtmp <--q */ cblas_dcopy(n, q, 1, qtmp, 1); double alpha = 1.0; double beta = 1.0; /*qtmp = H reaction +qtmp */ prodNumericsMatrix(m, n, alpha, H, reaction , beta, qtmp); if (M->storageType == 1) { beta = 0.0; assert(Global_MisInverse); /* globalVelocity = M^-1 qtmp */ prodNumericsMatrix(n, n, alpha, M, qtmp , beta, globalVelocity); } else if (M->storageType == 0) { int infoDGETRS = -1; cblas_dcopy(n, qtmp, 1, globalVelocity, 1); assert(Global_MisLU); DGETRS(LA_NOTRANS, n, 1, M->matrix0, n, Global_ipiv, globalVelocity , n, &infoDGETRS); assert(!infoDGETRS); } /* Compute current local velocity */ /* velocity <--b */ cblas_dcopy(m, b, 1, velocity, 1); if (H->storageType == 1) { /* velocity <-- H^T globalVelocity + velocity*/ beta = 1.0; prodSBM(n, m, alpha, Htrans, globalVelocity , beta, velocity); } else if (H->storageType == 0) { cblas_dgemv(CblasColMajor,CblasTrans, n, m, 1.0, H->matrix0 , n, globalVelocity , 1, 1.0, velocity, 1); } /* Loop through the contact points */ for (contact = 0 ; contact < nc ; ++contact) { /* (*local_solver)(contact,n,reaction,iparam,dparam); */ int pos = contact * 3; double normUT = sqrt(velocity[pos + 1] * velocity[pos + 1] + velocity[pos + 2] * velocity[pos + 2]); double an = 1.0; reaction[pos] -= an * (velocity[pos] + mu[contact] * normUT); reaction[pos + 1] -= an * velocity[pos + 1]; reaction[pos + 2] -= an * velocity[pos + 2]; projectionOnCone(&reaction[pos], mu[contact]); } /* int k; */ /* printf("\n"); */ /* for (k = 0 ; k < m; k++) printf("velocity[%i] = %12.8e \t \t reaction[%i] = %12.8e \n ", k, velocity[k], k , reaction[k]); */ /* for (k = 0 ; k < n; k++) printf("globalVelocity[%i] = %12.8e \t \n ", k, globalVelocity[k]); */ /* printf("\n"); */ /* **** Criterium convergence **** */ (*computeError)(problem, reaction , velocity, globalVelocity, tolerance, &error); if (verbose > 0) printf("----------------------------------- FC3D - NSGS - Iteration %i Error = %14.7e\n", iter, error); if (error < tolerance) hasNotConverged = 0; *info = hasNotConverged; } if (H->storageType == 1) { freeSBM(Htrans); } free(Htrans); free(qtmp); /* free(Global_ipiv); */ dparam[0] = tolerance; dparam[1] = error; /***** Free memory *****/ (*freeSolver)(problem); }
void fc3d_ExtraGradient(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]; /* Tolerance */ double tolerance = dparam[0]; double normq = 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 *)malloc(n * sizeof(double)); double * reactiontmp = (double *)malloc(n * sizeof(double)); double rho = 0.0, rho_k =0.0; int isVariable = 0; if (dparam[3] > 0.0) { rho = dparam[3]; if (verbose > 0) { printf("----------------------------------- FC3D - Extra Gradient (EG) - Fixed stepsize with rho = %14.7e \n", rho); } } else { /* Variable step in iterations*/ isVariable = 1; rho = -dparam[3]; if (verbose > 0) { printf("----------------------------------- FC3D - Extra Gradient (EG) - Variable stepsize with starting rho = %14.7e \n", rho); } } double alpha = 1.0; double beta = 1.0; /* Variable for Line_search */ int success =0; double error_k; int ls_iter = 0; int ls_itermax = 10; double tau=0.6, L= 0.9, Lmin =0.3, taumin=0.7; double a1=0.0, a2=0.0; double * reaction_k =0; double * velocity_k =0; if (isVariable) { reaction_k = (double *)malloc(n * sizeof(double)); velocity_k = (double *)malloc(n * sizeof(double)); } if (!isVariable) { /* double minusrho = -1.0*rho; */ while ((iter < itermax) && (hasNotConverged > 0)) { ++iter; /* reactiontmp <- reaction */ cblas_dcopy(n , reaction , 1 , reactiontmp, 1); /* velocitytmp <- q */ cblas_dcopy(n , q , 1 , velocitytmp, 1); prodNumericsMatrix(n, n, alpha, M, reactiontmp, beta, velocitytmp); // projection for each contact 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]); } cblas_dcopy(n , q , 1 , velocitytmp, 1); prodNumericsMatrix(n, n, alpha, M, reactiontmp, beta, velocitytmp); // projection for each contact 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]); reaction[pos] -= rho * (velocitytmp[pos] + mu[contact] * normUT); reaction[pos + 1] -= rho * velocitytmp[pos + 1]; reaction[pos + 2] -= rho * velocitytmp[pos + 2]; projectionOnCone(&reaction[pos], mu[contact]); } /* **** Criterium convergence **** */ fc3d_compute_error(problem, reaction , velocity, tolerance, options, normq, &error); if (options->callback) { options->callback->collectStatsIteration(options->callback->env, nc * 3, reaction, velocity, error, NULL); } if (verbose > 0) { printf("----------------------------------- FC3D - Extra Gradient (EG) - Iteration %i rho = %14.7e \tError = %14.7e\n", iter, rho, error); } if (error < tolerance) hasNotConverged = 0; *info = hasNotConverged; } } if (isVariable) { while ((iter < itermax) && (hasNotConverged > 0)) { ++iter; /* Store the error */ error_k = error; /* store the reaction at the beginning of the iteration */ cblas_dcopy(n , reaction , 1 , reaction_k, 1); /* velocity_k <- q */ cblas_dcopy(n , q , 1 , velocity_k, 1); /* velocity_k <- q + M * reaction_k */ beta = 1.0; prodNumericsMatrix(n, n, alpha, M, reaction_k, beta, velocity_k); ls_iter = 0 ; success =0; while (!success && (ls_iter < ls_itermax)) { rho_k = rho * pow(tau,ls_iter); /* projection for each contact */ for (contact = 0 ; contact < nc ; ++contact) { int pos = contact * nLocal; double normUT = sqrt(velocity_k[pos + 1] * velocity_k[pos + 1] + velocity_k[pos + 2] * velocity_k[pos + 2]); /* reaction[pos] = reaction_k[pos] - rho_k * (velocity_k[pos] + mu[contact] * normUT); */ /* reaction[pos + 1] = reaction_k[pos+1] - rho_k * velocity_k[pos + 1]; */ /* reaction[pos + 2] = reaction_k[pos+2] - rho_k * velocity_k[pos + 2]; */ reaction[pos] -= rho_k * (velocity_k[pos] + mu[contact] * normUT); reaction[pos + 1] -= rho_k * velocity_k[pos + 1]; reaction[pos + 2] -= rho_k * velocity_k[pos + 2]; projectionOnCone(&reaction[pos], mu[contact]); } /* velocity <- q + M * reaction */ beta = 1.0; cblas_dcopy(n , q , 1 , velocity, 1); prodNumericsMatrix(n, n, alpha, M, reaction, beta, velocity); /* velocitytmp <- velocity */ DEBUG_EXPR_WE( for (int i =0; i< 5 ; i++) { printf("reaction[%i]=%12.8e\t",i,reaction[i]); printf("velocity[%i]=F[%i]=%12.8e\n",i,i,velocity[i]); } ); cblas_dcopy(n, velocity, 1, velocitytmp , 1) ; /* velocitytmp <- modified velocity - velocity_k */ 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]); double normUT_k = sqrt(velocity_k[pos + 1] * velocity_k[pos + 1] + velocity_k[pos + 2] * velocity_k[pos + 2]); velocitytmp[pos] += mu[contact] * (normUT -normUT_k) ; } /* for (contact = 0 ; contact < nc ; ++contact) */ /* { */ /* int pos = contact * nLocal; */ /* double normUT = sqrt(velocity_k[pos + 1] * velocity_k[pos + 1] + velocity_k[pos + 2] * velocity_k[pos + 2]); */ /* velocity_k[pos] += mu[contact] * normUT; */ /* } */ cblas_daxpy(n, -1.0, velocity_k , 1, velocitytmp , 1) ; a1 = cblas_dnrm2(n, velocitytmp, 1); DEBUG_PRINTF("a1 = %12.8e\n", a1); /* reactiontmp <- reaction */ cblas_dcopy(n, reaction, 1, reactiontmp , 1) ; /* reactiontmp <- reaction - reaction_k */ cblas_daxpy(n, -1.0, reaction_k , 1, reactiontmp , 1) ; a2 = cblas_dnrm2(n, reactiontmp, 1) ; DEBUG_PRINTF("a2 = %12.8e\n", a2); 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",cblas_dnrm2(n, reaction, 1) ); */ /* printf("success = %i\n", success); */ ls_iter++; } /* velocitytmp <- M* reaction* q */ cblas_dcopy(n , q , 1 , velocitytmp, 1); prodNumericsMatrix(n, n, alpha, M, reaction, beta, velocitytmp); // projection for each contact 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]); reaction[pos] -= rho_k * (velocitytmp[pos] + mu[contact] * normUT); reaction[pos + 1] -= rho_k * velocitytmp[pos + 1]; reaction[pos + 2] -= rho_k * velocitytmp[pos + 2]; /* reaction[pos] = reaction_k[pos] - rho_k * (velocitytmp[pos] + mu[contact] * normUT); */ /* reaction[pos + 1] = reaction_k[pos+1] - rho_k * velocitytmp[pos + 1]; */ /* reaction[pos + 2] = reaction_k[pos+2] - rho_k * velocitytmp[pos + 2]; */ projectionOnCone(&reaction[pos], mu[contact]); } DEBUG_EXPR_WE( for (int i =0; i< 5 ; i++) { printf("reaction[%i]=%12.8e\t",i,reaction[i]); printf("velocity[%i]=F[%i]=%12.8e\n",i,i,velocity[i]); } ); /* **** Criterium convergence **** */ fc3d_compute_error(problem, reaction , velocity, tolerance, options, normq, &error); DEBUG_PRINTF("error = %12.8e\t error_k = %12.8e\n",error,error_k); /*Update rho*/ if ((rho_k*a1 < Lmin * a2) && (error < error_k)) { rho =rho_k/taumin; } else rho =rho_k; if (verbose > 0) { printf("----------------------------------- FC3D - Extra Gradient (EG) - Iteration %i rho = %14.7e \tError = %14.7e\n", iter, rho, error); } if (error < tolerance) hasNotConverged = 0; *info = hasNotConverged; }