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;
}
