void computeFz(double* z)
{
int incx = 1, incy = 1;
int size = sN + sM;
//F(z)=Mz+q
cblas_dcopy(size , sProblem->q , incx , sFz , incy);
prodNumericsMatrix(size, size, 1.0, sProblem->M, z, 1.0, sFz);
}
int soclcp_compute_error_v(SecondOrderConeLinearComplementarityProblem* problem, double *z , double *w, double tolerance, SolverOptions *options, double * error)
{
/* Checks inputs */
if(problem == NULL || z == NULL || w == NULL)
numericsError("soclcp_compute_error", "null input for problem and/or z and/or w");
/* Computes w = Mz + q */
int incx = 1, incy = 1;
int nc = problem->nc;
int n = problem->n;
double *mu = problem->mu;
double invmu = 0.0;
cblas_dcopy(n , problem->q , incx , z , incy); // z <-q
// Compute the current reaction
prodNumericsMatrix(n, n, 1.0, problem->M, w, 1.0, z);
*error = 0.;
double rho = 1.0;
for(int ic = 0 ; ic < nc ; ic++)
{
int dim = problem->coneIndex[ic+1]-problem->coneIndex[ic];
double * worktmp = (double *)malloc(dim*sizeof(double)) ;
int nic = problem->coneIndex[ic];
for (int i=0; i < dim; i++)
{
worktmp[i] = w[nic+i] - rho * z[nic+i];
}
invmu = 1.0 / mu[ic];
projectionOnSecondOrderCone(worktmp, invmu, dim);
for (int i=0; i < dim; i++)
{
worktmp[i] = w[nic+i] - worktmp[i];
*error += worktmp[i] * worktmp[i];
}
free(worktmp);
}
*error = sqrt(*error);
/* Computes error */
double normq = cblas_dnrm2(n , problem->q , incx);
*error = *error / (normq + 1.0);
if(*error > tolerance)
{
/* if (verbose > 0) printf(" Numerics - soclcp_compute_error_velocity failed: error = %g > tolerance = %g.\n",*error, tolerance); */
return 1;
}
else
return 0;
}
void Ftest(void * self, int n_unused, double *x, double *F)
{
VariationalInequality * vi = (VariationalInequality *) self;
Problems* pb = (Problems *)vi->env;
FrictionContactProblem * fc3d = pb->fc3d;
//frictionContact_display(fc3d);
int nc = fc3d->numberOfContacts;
int nLocal = fc3d->dimension;
int n = nc * nLocal;
cblas_dcopy(n , fc3d->q , 1 , F, 1);
prodNumericsMatrix(n, n, 1.0, fc3d->M, x, 1.0, F);
int contact =0;
for (contact = 0 ; contact < nc ; ++contact)
{
int pos = contact * nLocal;
double normUT = sqrt(F[pos + 1] * F[pos + 1] + F[pos + 2] * F[pos + 2]);
F[pos] += (fc3d->mu[contact] * normUT);
}
}
int lcp_compute_error(LinearComplementarityProblem* problem, double *z , double *w, double tolerance, double * error)
{
/* Checks inputs */
if (problem == NULL || z == NULL || w == NULL)
numerics_error("lcp_compute_error", "null input for problem and/or z and/or w");
/* Computes w = Mz + q */
int incx = 1, incy = 1;
unsigned int n = problem->size;
cblas_dcopy(n , problem->q , incx , w , incy); // w <-q
prodNumericsMatrix(n, n, 1.0, problem->M, z, 1.0, w);
double normq = cblas_dnrm2(n , problem->q , incx);
lcp_compute_error_only(n, z, w, error);
*error = *error / (normq + 1.0); /* Need some comments on why this is needed */
if (*error > tolerance)
{
if (verbose > 0) printf(" Numerics - lcp_compute_error : error = %g > tolerance = %g.\n", *error, tolerance);
return 1;
}
else
return 0;
}
void Function_VI_FC3D(void * self, int n_notused, double *x, double *F)
{
DEBUG_PRINT("Function_VI_FC3D(void * self, double *x, double *F)\n")
VariationalInequality * vi = (VariationalInequality *) self;
FrictionContactProblem_as_VI* pb = (FrictionContactProblem_as_VI*)vi->env;
FrictionContactProblem * fc3d = pb->fc3d;
//frictionContact_display(fc3d);
int nLocal = fc3d->dimension;
int n = fc3d->numberOfContacts * fc3d->dimension;
cblas_dcopy(n , fc3d->q , 1 , F, 1);
prodNumericsMatrix(n, n, 1.0, fc3d->M, x, 1.0, F);
int contact =0;
for (contact = 0 ; contact < fc3d->numberOfContacts ; ++contact)
{
double normUT = sqrt(F[contact * nLocal + 1] * F[contact * nLocal + 1]
+ F[contact * nLocal + 2] * F[contact * nLocal + 2]);
F[contact * nLocal] += (fc3d->mu[contact] * normUT);
}
}
/*
* (input) double *z : size n+m
* (output)double *w : size n+m
*
*
*/
int mlcp_compute_error(MixedLinearComplementarityProblem* problem, double *z, double *w, double tolerance, double * error)
{
/* Checks inputs */
if (problem == NULL || z == NULL || w == NULL)
numerics_error("mlcp_compute_error", "null input for problem and/or z and/or w");
int param = 1;
int NbLines = problem->M->size0; /* Equalities */
int n = problem->n; /* Equalities */
int m = problem->m; /* Inequalities */
int incx = 1, incy = 1;
/* Computation of w: depends on the way the problem is written */
/* Problem in the form (M,q) */
if (problem->isStorageType1)
{
if (problem->M == NULL)
numerics_error("mlcp_compute_error", "null input for M");
/* Computes w = Mz + q */
cblas_dcopy(NbLines , problem->q , incx , w , incy);
prodNumericsMatrix(problem->M->size1, problem->M->size0, 1.0, problem->M, z, 1.0, w);
}
/* Problem in the form ABCD */
else //if (problem->isStorageType2)
{
/* Checks inputs */
if (problem->A == NULL || problem->B == NULL || problem->C == NULL || problem->D == NULL)
{
numerics_error("mlcp_compute_error: ", "null input for A, B, C or D");
}
/* Links to problem data */
double *a = &problem->q[0];
double *b = &problem->q[NbLines - m];
double *A = problem->A;
double *B = problem->B;
double *C = problem->C;
double *D = problem->D;
/* Compute "equalities" part, we = Au + Cv + a - Must be equal to 0 */
cblas_dcopy(NbLines - m , a , incx , w , incy); // we = w[0..n-1] <-- a
cblas_dgemv(CblasColMajor,CblasNoTrans , NbLines - m, n , 1.0 , A , NbLines - m , &z[0] , incx , 1.0 , w , incy); // we <-- A*u + we
cblas_dgemv(CblasColMajor,CblasNoTrans , NbLines - m, m , 1.0 , C , NbLines - m , &z[n] , incx , 1.0 , w , incy); // we <-- C*v + we
/* Computes part which corresponds to complementarity */
double * pwi = w + NbLines - m; // No copy!!
cblas_dcopy(m , b , incx , pwi , incy); // wi = w[n..m] <-- b
// following int param, we recompute the product wi = Du+BV +b and we = Au+CV +a
// The test is then more severe if we compute w because it checks that the linear equation is satisfied
if (param == 1)
{
cblas_dgemv(CblasColMajor,CblasNoTrans , m, n , 1.0 , D , m , &z[0] , incx , 1.0 , pwi , incy); // wi <-- D*u+ wi
cblas_dgemv(CblasColMajor,CblasNoTrans , m , m , 1.0 , B , m , &z[n] , incx , 1.0 , pwi , incy); // wi <-- B*v + wi
}
}
/* Error on equalities part */
double error_e = 0;
/* Checks complementarity (only for rows number n to size) */
double error_i = 0.;
double zi, wi;
double *q = problem->q;
double norm_e = 1;
double norm_i = 1;
if (problem->blocksRows)
{
int numBlock = 0;
while (problem->blocksRows[numBlock] < n + m)
{
if (!problem->blocksIsComp[numBlock])
{
error_e += cblas_dnrm2(problem->blocksRows[numBlock + 1] - problem->blocksRows[numBlock], w + problem->blocksRows[numBlock] , incx);
norm_e += cblas_dnrm2(problem->blocksRows[numBlock + 1] - problem->blocksRows[numBlock], q + problem->blocksRows[numBlock] , incx);
}
else
{
for (int numLine = problem->blocksRows[numBlock]; numLine < problem->blocksRows[numBlock + 1] ; numLine++)
{
zi = z[numLine];
wi = w[numLine];
if (zi < 0.0)
{
error_i += -zi;
if (wi < 0.0) error_i += zi * wi;
}
if (wi < 0.0) error_i += -wi;
if ((zi > 0.0) && (wi > 0.0)) error_i += zi * wi;
}
norm_i += cblas_dnrm2(problem->blocksRows[numBlock + 1] - problem->blocksRows[numBlock], w + problem->blocksRows[numBlock] , incx);
}
numBlock++;
}
}
else
{
printf("WARNING, DEPRECATED MLCP API\n");
/* Error on equalities part */
error_e = cblas_dnrm2(NbLines - m , w , incx);;
/* Checks complementarity (only for rows number n to size) */
error_i = 0.;
for (int i = 0 ; i < m ; i++)
{
zi = z[n + i];
wi = w[(NbLines - m) + i];
if (zi < 0.0)
{
error_i += -zi;
if (wi < 0.0) error_i += zi * wi;
}
if (wi < 0.0) error_i += -wi;
if ((zi > 0.0) && (wi > 0.0)) error_i += zi * wi;
}
/* Computes error */
norm_i += cblas_dnrm2(m , q + NbLines - m , incx);
norm_e += cblas_dnrm2(NbLines - m , q , incx);
}
if (error_i / norm_i >= error_e / norm_e)
{
*error = error_i / (1.0 + norm_i);
}
else
{
*error = error_e / (1.0 + norm_e);
}
if (*error > tolerance)
{
/*if (isVerbose > 0) printf(" Numerics - mlcp_compute_error failed: error = %g > tolerance = %g.\n",*error, tolerance);*/
if (verbose)
printf(" Numerics - mlcp_compute_error failed: error = %g > tolerance = %g.\n", *error, tolerance);
/* displayMLCP(problem);*/
return 1;
}
else
{
if (verbose > 0) printf("Siconos/Numerics: mlcp_compute_error: Error evaluation = %g \n", *error);
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 soclcp_compute_error(
SecondOrderConeLinearComplementarityProblem* problem,
double *z , double *w, double tolerance,
SolverOptions * options, double * error)
{
assert(problem);
assert(z);
assert(w);
assert(error);
/* Computes w = Mz + q */
int incx = 1, incy = 1;
int nc = problem->nc;
double *mu = problem->mu;
int n = problem->n;
cblas_dcopy(n , problem->q , incx , w , incy); // w <-q
// Compute the current velocity
prodNumericsMatrix(n, n, 1.0, problem->M, z, 1.0, w);
/* for (int i=0; i < n ; i++ ) */
/* { */
/* printf("w[%i]=%e\t\t\t",i,w[i]); */
/* printf("z[%i]=%e\n",i,z[i]); */
/* } */
/* printf("\n"); */
*error = 0.;
int ic;
int dim;
unsigned int dim_max;
for (int i =0; i <nc; i++)
{
dim_max=max(dim_max,problem->coneIndex[i+1]-problem->coneIndex[i]);
}
double *worktmp = (double *)calloc(dim_max,sizeof(double));
for(ic = 0 ; ic < nc ; ic++)
{
dim = problem->coneIndex[ic+1]- problem->coneIndex[ic];
soclcp_unitary_compute_and_add_error(z + problem->coneIndex[ic],
w + problem->coneIndex[ic],
dim , mu[ic], error, worktmp);
/* for (int i=0; i < dim; i++ ) */
/* { */
/* printf("-- w[%i]=%e\t\t\t",i,(w + problem->coneIndex[ic])[i]); */
/* printf("z[%i]=%e\n",i,(z + problem->coneIndex[ic])[i]); */
/* } */
}
free(worktmp);
*error = sqrt(*error);
/* Computes error */
double normq = cblas_dnrm2(n , problem->q , incx);
DEBUG_PRINTF("normq = %12.8e\n", normq);
*error = *error / (normq + 1.0);
if(*error > tolerance)
{
if(verbose > 1)
printf(" Numerics - soclcp_compute_error: error = %g > tolerance = %g.\n",
*error, tolerance);
return 1;
}
else
return 0;
}
int test_prodNumericsMatrix(NumericsMatrix** MM)
{
NumericsMatrix* M1 = MM[0];
NumericsMatrix* M2 = MM[1];
NumericsMatrix* M3 = MM[2];
NumericsMatrix* M4 = MM[3];
printf("== Numerics tests: prodNumericsMatrix(NumericsMatrix,vector) == \n");
int i , n = M1->size1, m = 4;
double * x = (double *)malloc(n * sizeof(double));
double * x2 = (double *)malloc(m * sizeof(double));
double alpha = 2.3, beta = 1.9;
double * yref = (double *)malloc(n * sizeof(double));
double * yref2 = (double *)malloc(n * sizeof(double));;
double * y = (double *)malloc(n * sizeof(double));
double * y2 = (double *)malloc(n * sizeof(double));
for (i = 0; i < n; i++)
{
x[i] = i + 1.0;
yref[i] = 0.1 * i;
yref2[i] = 0.1 * i;
y[i] = yref[i];
y2[i] = yref2[i];
}
x2[0] = 0;
x2[1] = 0;
x2[2] = 0;
x2[3] = 0;
int incx = 1, incy = 1;
cblas_dgemv(CblasColMajor, CblasNoTrans, n, n, alpha, M1->matrix0, n, x, incx, beta, yref, incy);
prodNumericsMatrix(n, n, alpha, M1, x, beta, y);
double tol = 1e-12;
int info = 0;
for (i = 0; i < n; i++)
{
if (fabs(y[i] - yref[i]) > tol) info = 1;
// printf("%lf\n", fabs(y[i]-yref[i]));
}
if (info == 0)
printf("Step 0 ( y = alpha*A*x + beta*y, double* storage) ok ...\n");
else
printf("Step 0 ( y = alpha*A*x + beta*y, double* storage) failed ...\n");
cblas_dgemv(CblasColMajor, CblasNoTrans, n, m, alpha, M3->matrix0, n, x2, incx, beta, yref2, incy);
prodNumericsMatrix(m, n, alpha, M3, x2, beta, y2);
for (i = 0; i < n; i++)
{
if (fabs(y2[i] - yref2[i]) > tol) info = 1;
/* printf("%lf\n", fabs(y2[i]-yref2[i])); */
/* printf("%lf\n",y2[i]); */
/* printf("%lf\n",yref2[i]); */
}
if (info == 0)
printf("Step 1 ( y = alpha*A*x + beta*y, double* storage, non square) ok ...\n");
else
printf("Step 1 ( y = alpha*A*x + beta*y, double* storage, non square) failed ...\n");
/* Sparse ... */
for (i = 0; i < n; i++)
{
y[i] = 0.1 * i;
y2[i] = 0.1 * i;
}
prodNumericsMatrix(n, n, alpha, M2, x, beta, y);
for (i = 0; i < n; i++)
{
if (fabs(y[i] - yref[i]) > tol) info = 1;
/* printf("%lf\n", fabs(y[i]-yref[i])); */
/* printf("%lf\n", y[i]); */
}
if (info == 0)
printf("Step 2 ( y = alpha*A*x + beta*y, sparse storage) ok ...\n");
else
printf("Step 2 ( y = alpha*A*x + beta*y, sparsestorage) failed ...\n");
prodNumericsMatrix(m, n, alpha, M4, x2, beta, y2);
for (i = 0; i < n; i++)
{
if (fabs(y2[i] - yref2[i]) > tol) info = 1;
/* printf("%lf\n", fabs(y2[i]-yref2[i])); */
/* printf("%lf\n",y2[i]); */
/* printf("%lf\n",yref2[i]); */
}
if (info == 0)
printf("Step 3 ( y = alpha*A*x + beta*y, sparse storage, non square) ok ...\n");
else
printf("Step 3 ( y = alpha*A*x + beta*y, sparsestorage, non square) failed ...\n");
free(x);
free(x2);
free(y);
free(y2);
free(yref);
free(yref2);
printf("== End of test prodNumericsMatrix(NumericsMatrix,vector), result = %d\n", info);
return info;
}
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 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;
}
int fc2d_compute_error(FrictionContactProblem* problem, double *z , double *w, double tolerance, double * error)
{
/* Checks inputs */
if (! problem || ! z || ! w)
numerics_error("fc2d_compute_error", "null input for problem and/or z and/or w");
int nc = problem->numberOfContacts;
int n = nc * 2;
int ic, iN, iT;
double *mu = problem->mu;
double tmp[2];
double normT;
cblas_dcopy(n, problem->q, 1, w, 1); // w <-q
prodNumericsMatrix(n, n, 1.0, problem->M, z, 1.0, w);
*error = 0.;
/* Num. Methods For Nonsmooth Dynamics, A.13 P 528 */
/* DesaxceFeng98 */
/* K* -) x _|_ y (- K <=> x = projK(x-rho.y) for all rho>0 */
for (ic = 0, iN = 0, iT = 1 ; ic < nc ; ++ic , ++iN, ++iN, ++iT, ++iT)
{
/* Compute the modified local velocity */
tmp[0] = z[iN] - (w[iN] + mu[ic] * fabs(w[iT])); /* rho=1 */
tmp[1] = z[iT] - w[iT]; /* rho=1 */
/* projection */
normT = fabs(tmp[1]);
if (mu[ic]*normT <= -tmp[0])
{
tmp[0] = 0.;
tmp[1] = 0.;
}
else if (normT > mu[ic]*tmp[0])
{
/* solve([sqrt((r1-mu*ra)^2+(r0-ra)^2)=abs(mu*r0-r1)/sqrt(mu*mu+1)],[ra]) */
tmp[0] = (mu[ic] * normT + tmp[0]) / (mu[ic] * mu[ic] + 1);
tmp[1] = mu[ic] * tmp[0] * SGN(tmp[1]);
}
tmp[0] = z[iN] - tmp[0];
tmp[1] = z[iT] - tmp[1];
*error += tmp[0] * tmp[0] + tmp[1] * tmp[1];
}
*error = sqrt(*error);
*error /= (cblas_dnrm2(n, problem->q, 1) + 1.0);
if (*error > tolerance)
{
if (verbose > 1) printf(" Numerics - fc2d_compute_error failed: error = %g > tolerance = %g.\n", *error, tolerance);
return 1;
}
else
return 0;
}
