/* compute psi function */
void ACPsi(
GlobalFrictionContactProblem* problem,
AlartCurnierFun3x3Ptr computeACFun3x3,
double *globalVelocity,
double *reaction,
double *velocity,
double *rho,
double *psi)
{
assert(problem->H->size1 == problem->dimension * problem->numberOfContacts);
unsigned int localProblemSize = problem->H->size1;
unsigned int ACProblemSize = sizeOfPsi(NM_triplet(problem->M),
NM_triplet(problem->H));
unsigned int globalProblemSize = problem->M->size0;
/* psi <-
compute -problem->M * globalVelocity + problem->H * reaction + problem->q
... */
cblas_dscal(ACProblemSize, 0., psi, 1);
cblas_dcopy(globalProblemSize, problem->q, 1, psi, 1);
NM_gemv(1., problem->H, reaction, 1, psi);
NM_gemv(-1., problem->M, globalVelocity, 1, psi);
/* psi + globalProblemSize <-
compute -velocity + trans(problem->H) * globalVelocity + problem->b
... */
cblas_daxpy(localProblemSize, -1., velocity, 1, psi + globalProblemSize, 1);
cblas_daxpy(localProblemSize, 1, problem->b, 1, psi + globalProblemSize, 1);
NM_tgemv(1., problem->H, globalVelocity, 1, psi + globalProblemSize);
/* compute AC function */
fc3d_AlartCurnierFunction(localProblemSize,
computeACFun3x3,
reaction,
velocity, problem->mu, rho,
psi+globalProblemSize+
problem->H->size1,
NULL, NULL);
}
static void FC3D_compute_F(void* data_opaque, double* reaction, double* velocity)
{
FrictionContactProblem* problem = ((FC3D_Newton_data*) data_opaque)->problem;
// velocity <- M*reaction + qfree
cblas_dcopy(problem->M->size1, problem->q, 1, velocity, 1);
NM_gemv(1., problem->M, reaction, 1., velocity);
}
void Function_VI_SOCLCP(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;
SecondOrderConeLinearComplementarityProblem_as_VI* pb = (SecondOrderConeLinearComplementarityProblem_as_VI*)vi->env;
SecondOrderConeLinearComplementarityProblem * soclcp = pb->soclcp;
//frictionContact_display(fc3d);
int n = soclcp->n;
cblas_dcopy(n , soclcp->q , 1 , F, 1);
NM_gemv(1.0, soclcp->M, x, 1.0, F);
}
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)
numerics_error("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->tau;
double invmu = 0.0;
cblas_dcopy(n , problem->q , incx , z , incy); // z <-q
// Compute the current reaction
NM_gemv(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 norm_q = cblas_dnrm2(n , problem->q , incx);
*error = *error / (norm_q + 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;
}
/* compute psi function */
void ACPsi(
GlobalFrictionContactProblem* problem,
AlartCurnierFun3x3Ptr computeACFun3x3,
double *globalVelocity,
double *reaction,
double *velocity,
double *rho,
double *psi)
{
assert(problem->H->size1 == problem->dimension * problem->numberOfContacts);
unsigned int m = problem->H->size1;
unsigned int n = problem->M->size0;
unsigned int problem_size = n + 2*m ;
cblas_dscal(problem_size, 0., psi, 1);
/* -problem->M * globalVelocity + problem->H * reaction + problem->q ==> psi */
cblas_dscal(problem_size, 0., psi, 1);
cblas_dcopy(n, problem->q, 1, psi, 1);
NM_gemv(1., problem->H, reaction, 1, psi);
NM_gemv(-1., problem->M, globalVelocity, 1, psi);
/* -velocity + trans(problem->H) * globalVelocity + problem->b ==> psi + n */
cblas_daxpy(m, -1., velocity, 1, psi + n, 1);
cblas_daxpy(m, 1, problem->b, 1, psi + n, 1);
NM_tgemv(1., problem->H, globalVelocity, 1, psi + n);
/* compute AC function */
fc3d_AlartCurnierFunction(m,
computeACFun3x3,
reaction,
velocity, problem->mu, rho,
psi+n+m,
NULL, NULL);
}
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 fc3d_Panagiotopoulos_FixedPoint(FrictionContactProblem* problem, double *reaction, double *velocity, int* info, SolverOptions* options)
{
/* verbose=1; */
/* int and double parameters */
int* iparam = options->iparam;
double* dparam = options->dparam;
/* Number of contacts */
int nc = problem->numberOfContacts;
/* Maximum number of iterations */
int itermax = iparam[SICONOS_IPARAM_MAX_ITER];
/* Tolerance */
double tolerance = dparam[SICONOS_DPARAM_TOL];
double norm_q = cblas_dnrm2(nc*3 , problem->q , 1);
if (options->numberOfInternalSolvers < 1)
{
numerics_error("fc3d_TrescaFixedpoint", "The Tresca Fixed Point method needs options for the internal solvers, options[0].numberOfInternalSolvers should be >1");
}
SolverOptions * internalsolver_options = options->internalSolvers;
if (verbose) solver_options_print(options);
/***** Fixed Point Iterations *****/
int iter = 0; /* Current iteration number */
double error = 1.; /* Current error */
int hasNotConverged = 1;
normalInternalSolverPtr internalsolver_normal;
tangentInternalSolverPtr internalsolver_tangent;
options->dWork = (double *) malloc(nc * sizeof(double));
options->dWorkSize = nc;
double * mu = options->dWork;
internalsolver_options->dWork = options->dWork;
double * r_n = (double *) malloc(nc * sizeof(double));
double * r_t = (double *) malloc(2* nc * sizeof(double));
for (int contact = 0 ; contact < nc; contact ++)
{
r_n[contact] = reaction[contact*3];
r_t[2*contact] = reaction[contact*3+1];
r_t[2*contact+1] = reaction[contact*3+2];
}
SplittedFrictionContactProblem * splitted_problem = (SplittedFrictionContactProblem *)malloc(sizeof(SplittedFrictionContactProblem));
createSplittedFrictionContactProblem(problem, splitted_problem);
LinearComplementarityProblem* normal_lcp_problem;
ConvexQP * tangent_cqp;
if (options->numberOfInternalSolvers !=2)
numerics_error("fc3d_Panagiotopoulos_FixedPoint", " the solver requires 2 internal solver");
if (internalsolver_options[0].solverId == SICONOS_LCP_PGS||
internalsolver_options[0].solverId == SICONOS_LCP_CONVEXQP_PG)
{
normal_lcp_problem = (LinearComplementarityProblem*)malloc(sizeof(LinearComplementarityProblem));
normal_lcp_problem->size = nc;
normal_lcp_problem->M = splitted_problem->M_nn;
/* for (int contact = 0 ; contact < nc; contact ++) */
/* { */
/* problem->mu[contact] =0.0; */
/* } */
/* splitted_problem->M_nn->matrix0 =(double *)malloc(splitted_problem->M_nn->size0*splitted_problem->M_nn->size1*sizeof(double)); */
/* SBM_to_dense(splitted_problem->M_nn->matrix1, splitted_problem->M_nn->matrix0); */
/* splitted_problem->M_nn->storageType=NM_DENSE; */
normal_lcp_problem->q = (double *)malloc(nc*sizeof(double));
}
else
{
numerics_error("fc3d_Panagiotopoulos_FixedPoint", "Unknown internal solver for the normal part.");
}
if (internalsolver_options[1].solverId == SICONOS_CONVEXQP_PG ||
internalsolver_options[1].solverId == SICONOS_CONVEXQP_VI_FPP||
internalsolver_options[1].solverId == SICONOS_CONVEXQP_VI_EG)
{
tangent_cqp = (ConvexQP *)malloc(sizeof(ConvexQP));
tangent_cqp->M = splitted_problem->M_tt;
tangent_cqp->q = (double *) malloc(2* nc * sizeof(double));
tangent_cqp->ProjectionOnC = &Projection_ConvexQP_FC3D_Disk;
tangent_cqp->A=NULL;
tangent_cqp->b= NULL;
FrictionContactProblem_as_ConvexQP *fc3d_as_cqp= (FrictionContactProblem_as_ConvexQP*)malloc(sizeof(FrictionContactProblem_as_ConvexQP));
tangent_cqp->env = fc3d_as_cqp ;
tangent_cqp->size = nc*2;
/*set the norm of the VI to the norm of problem->q */
double norm_q_t = cblas_dnrm2(nc*2 , splitted_problem->q_t , 1);
tangent_cqp->normConvexQP= norm_q_t;
tangent_cqp->istheNormConvexQPset=1;
fc3d_as_cqp->cqp = tangent_cqp;
fc3d_as_cqp->fc3d = problem;
fc3d_as_cqp->options = options;
}
else
{
numerics_error("fc3d_Panagiotopoulos_FixedPoint", "Unknown internal solver for the tangent part.");
}
if (internalsolver_options[0].solverId == SICONOS_LCP_PGS)
{
if (verbose > 0)
printf(" ========================== Call LCP_PGS solver for Friction-Contact 3D problem ==========================\n");
internalsolver_normal = &lcp_pgs;
}
else if (internalsolver_options[0].solverId == SICONOS_LCP_CONVEXQP_PG)
{
if (verbose > 0)
printf(" ========================== Call LCP_CONVEX_QP solver for Friction-Contact 3D problem ==========================\n");
internalsolver_normal = &lcp_ConvexQP_ProjectedGradient;
}
else
{
numerics_error("fc3d_Panagiotopoulos_FixedPoint", "Unknown internal solver for the normal part.");
}
if (internalsolver_options[1].solverId == SICONOS_CONVEXQP_PG)
{
if (verbose > 0)
printf(" ========================== Call SICONOS_CONVEX_QP solver for Friction-Contact 3D problem ==========================\n");
internalsolver_tangent = &convexQP_ProjectedGradient;
}
else if (internalsolver_options[1].solverId == SICONOS_CONVEXQP_VI_FPP ||
internalsolver_options[1].solverId == SICONOS_CONVEXQP_VI_EG )
{
if (verbose > 0)
printf(" ========================== Call SICONOS_CONVEX_VI_FPP solver for Friction-Contact 3D problem ==========================\n");
internalsolver_tangent = &convexQP_VI_solver;
}
int cumul_internal=0;
//verbose=1;
while ((iter < itermax) && (hasNotConverged > 0))
{
++iter;
fc3d_set_internalsolver_tolerance(problem,options,&internalsolver_options[0], error);
/* ----------------- */
/* normal resolution */
/* ----------------- */
/* compute the rhs of the normal problem */
cblas_dcopy(nc , splitted_problem->q_n , 1 , normal_lcp_problem->q, 1);
NM_gemv(1.0, splitted_problem->M_nt, r_t, 1.0, normal_lcp_problem->q);
(*internalsolver_normal)(normal_lcp_problem, r_n , velocity , info , &internalsolver_options[0]);
cumul_internal += internalsolver_options[0].iparam[SICONOS_IPARAM_ITER_DONE];
for (int contact = 0 ; contact < nc; contact ++)
{
reaction[contact*3]= r_n[contact];
}
fc3d_compute_error(problem, reaction , velocity, tolerance, options, norm_q, &error);
if (error < tolerance)
{
hasNotConverged = 0;
}
else
{
/* ------------------ */
/* tangent resolution */
/* ------------------ */
fc3d_set_internalsolver_tolerance(problem,options,&internalsolver_options[1], error);
/* compute the rhs of the tangent problem */
cblas_dcopy(2*nc , splitted_problem->q_t, 1 , tangent_cqp->q, 1);
NM_gemv(1.0, splitted_problem->M_tn, r_n, 1.0, tangent_cqp->q);
/* Compute the value of the initial value friction threshold*/
for (int ic = 0 ; ic < nc ; ic++) mu[ic] = fmax(0.0, problem->mu[ic] * reaction [ic * 3]);
/* if (verbose>0) */
/* printf("norm of mu = %10.5e \n", cblas_dnrm2(nc , mu , 1)); */
fc3d_compute_error(problem, reaction , velocity, tolerance, options, norm_q, &error);
(*internalsolver_tangent)(tangent_cqp, r_t , velocity , info , &internalsolver_options[1]);
cumul_internal += internalsolver_options->iparam[SICONOS_IPARAM_ITER_DONE];
for (int contact = 0 ; contact < nc; contact ++)
{
reaction[contact*3+1]= r_t[2*contact];
reaction[contact*3+2]= r_t[2*contact+1];
}
/* **** 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 (error < tolerance) hasNotConverged = 0;
}
*info = hasNotConverged;
if (verbose > 0)
{
if (hasNotConverged)
{
printf("--------------- FC3D - PFP - Iteration %i error = %14.7e > %10.5e\n", iter, error, tolerance);
}
else
{
printf("--------------- FC3D - PFP - Iteration %i error = %14.7e < %10.5e\n", iter, error, tolerance);
printf("--------------- FC3D - PFP - # Internal iteration = %i\n", cumul_internal);
}
}
}
free(options->dWork);
options->dWork = NULL;
internalsolver_options->dWork = NULL;
if (internalsolver_options->internalSolvers != NULL)
internalsolver_options->internalSolvers->dWork = NULL;
dparam[SICONOS_DPARAM_RESIDU] = error;
iparam[SICONOS_IPARAM_ITER_DONE] = iter;
}
void fc3d_nonsmooth_Newton_solvers_solve(fc3d_nonsmooth_Newton_solvers* equation,
double* reaction,
double* velocity,
int* info,
SolverOptions* options)
{
assert(equation);
FrictionContactProblem* problem = equation->problem;
assert(problem);
assert(reaction);
assert(velocity);
assert(info);
assert(options);
assert(problem->dimension == 3);
assert(options->iparam);
assert(options->dparam);
assert(problem->q);
assert(problem->mu);
assert(problem->M);
assert(problem->M->matrix0 || problem->M->matrix1 || problem->M->matrix2);
assert(!options->iparam[4]); // only host
unsigned int problemSize = 3 * problem->numberOfContacts;
unsigned int iter = 0;
unsigned int itermax = options->iparam[0];
unsigned int erritermax = options->iparam[7];
int nzmax;
if (problem->M->storageType == NM_DENSE)
{
nzmax = problemSize * problemSize;
}
else
{
nzmax = options->iparam[3];
}
assert(itermax > 0);
assert(nzmax > 0);
double tolerance = options->dparam[0];
assert(tolerance > 0);
if (verbose > 0)
printf("------------------------ FC3D - _nonsmooth_Newton_solversSolve - Start with tolerance = %g\n", tolerance);
unsigned int _3problemSize = 3 * problemSize;
double normq = cblas_dnrm2(problemSize , problem->q , 1);
void *buffer;
if (!options->dWork)
{
buffer = malloc((11 * problemSize) * sizeof(double)); // F(1),
// tmp1(1),
// tmp2(1),
// tmp3(1),
// A(3),
// B(3), rho
}
else
{
buffer = options->dWork;
}
double *F = (double *) buffer;
double *tmp1 = (double *) F + problemSize;
double *tmp2 = (double *) tmp1 + problemSize;
double *tmp3 = (double *) tmp2 + problemSize;
double *Ax = tmp3 + problemSize;
double *Bx = Ax + _3problemSize;
double *rho = Bx + _3problemSize;
NumericsMatrix *AWpB, *AWpB_backup;
if (!options->dWork)
{
AWpB = createNumericsMatrix(problem->M->storageType,
problem->M->size0, problem->M->size1);
AWpB_backup = createNumericsMatrix(problem->M->storageType,
problem->M->size0, problem->M->size1);
}
else
{
AWpB = (NumericsMatrix*) (rho + problemSize);
AWpB_backup = (NumericsMatrix*) (AWpB + sizeof(NumericsMatrix*));
}
/* just for allocations */
NM_copy(problem->M, AWpB);
if (problem->M->storageType != NM_DENSE)
{
switch(options->iparam[13])
{
case 0:
{
NM_linearSolverParams(AWpB)->solver = NS_CS_LUSOL;
break;
}
case 1:
{
NM_linearSolverParams(AWpB)->solver = NS_MUMPS;
#ifdef HAVE_MPI
assert (options->solverData);
if ((MPI_Comm) options->solverData == MPI_COMM_NULL)
{
options->solverData = NM_MPI_com(MPI_COMM_NULL);
}
else
{
NM_MPI_com((MPI_Comm) options->solverData);
}
#endif
break;
}
default:
{
numerics_error("fc3d_nonsmooth_Newton_solvers_solve", "Unknown linear solver.\n");
}
}
}
// compute rho here
for (unsigned int i = 0; i < problemSize; ++i) rho[i] = options->dparam[3];
// velocity <- M*reaction + qfree
cblas_dcopy(problemSize, problem->q, 1, velocity, 1);
NM_gemv(1., problem->M, reaction, 1., velocity);
double linear_solver_residual=0.0;
while (iter++ < itermax)
{
equation->function(equation->data,
problemSize,
reaction, velocity, equation->problem->mu,
rho,
F, Ax, Bx);
// AW + B
computeAWpB(Ax, problem->M, Bx, AWpB);
cblas_dcopy_msan(problemSize, F, 1, tmp1, 1);
cblas_dscal(problemSize, -1., tmp1, 1);
/* Solve: AWpB X = -F */
NM_copy(AWpB, AWpB_backup);
int lsi = NM_gesv(AWpB, tmp1);
/* NM_copy needed here */
NM_copy(AWpB_backup, AWpB);
if (lsi)
{
if (verbose > 0)
{
numerics_warning("fc3d_nonsmooth_Newton_solvers_solve",
"warning! linear solver exit with code = %d\n", lsi);
}
}
if (verbose > 0)
{
cblas_dcopy_msan(problemSize, F, 1, tmp3, 1);
NM_gemv(1., AWpB, tmp1, 1., tmp3);
linear_solver_residual = cblas_dnrm2(problemSize, tmp3, 1);
/* fprintf(stderr, "fc3d esolve: linear equation residual = %g\n", */
/* cblas_dnrm2(problemSize, tmp3, 1)); */
/* for the component wise scaled residual: cf mumps &
* http://www.netlib.org/lapack/lug/node81.html */
}
// line search
double alpha = 1;
int info_ls = 0;
cblas_dcopy_msan(problemSize, tmp1, 1, tmp3, 1);
switch (options->iparam[11])
{
case -1:
/* without line search */
info_ls = 1;
break;
case 0:
/* Goldstein Price */
info_ls = globalLineSearchGP(equation, reaction, velocity, problem->mu, rho, F, Ax, Bx, problem->M, problem->q, AWpB, tmp1, tmp2, &alpha, options->iparam[12]);
break;
case 1:
/* FBLSA */
info_ls = frictionContactFBLSA(equation, reaction, velocity, problem->mu, rho, F, Ax, Bx,
problem->M, problem->q, AWpB, tmp1, tmp2, &alpha, options->iparam[12]);
break;
default:
{
numerics_error("fc3d_nonsmooth_Newton_solvers_solve",
"Unknown line search option.\n");
}
}
if (!info_ls)
// tmp2 should contains the reaction iterate of the line search
// for GP this should be the same as cblas_daxpy(problemSize, alpha, tmp1, 1, reaction, 1);
cblas_dcopy(problemSize, tmp2, 1, reaction, 1);
else
cblas_daxpy(problemSize, 1., tmp3, 1., reaction, 1);
// velocity <- M*reaction + qfree
cblas_dcopy(problemSize, problem->q, 1, velocity, 1);
NM_gemv(1., problem->M, reaction, 1., velocity);
options->dparam[1] = INFINITY;
if (!(iter % erritermax))
{
fc3d_compute_error(problem, reaction, velocity,
// fc3d_FischerBurmeister_compute_error(problem, reaction, velocity,
tolerance, options, normq, &(options->dparam[1]));
DEBUG_EXPR_WE(equation->function(equation->data, problemSize,
reaction, velocity, equation->problem->mu, rho,
F, NULL, NULL));
DEBUG_EXPR_WE(assert((cblas_dnrm2(problemSize, F, 1)
/ (1 + cblas_dnrm2(problemSize, problem->q, 1)))
<= (10 * options->dparam[1] + 1e-15)));
}
if (verbose > 0)
{
equation->function(equation->data, problemSize,
reaction, velocity, equation->problem->mu, rho,
F, NULL, NULL);
printf(" ---- fc3d_nonsmooth_Newton_solvers_solve: iteration %d : , linear solver residual =%g, residual=%g, ||F||=%g\n", iter, linear_solver_residual, options->dparam[1],cblas_dnrm2(problemSize, F, 1));
}
if (options->callback)
{
options->callback->collectStatsIteration(options->callback->env, problemSize, reaction, velocity,
options->dparam[1], NULL);
}
if (isnan(options->dparam[1]))
{
if (verbose > 0)
{
printf(" fc3d_nonsmooth_Newton_solvers_solve: iteration %d : computed residual is not a number, stop.\n", iter);
}
info[0] = 2;
break;
}
if (options->dparam[1] < tolerance)
{
info[0] = 0;
break;
}
}
if (verbose > 0)
{
if (!info[0])
printf("------------------------ FC3D - NSN - convergence after %d iterations, residual : %g < %g \n", iter, options->dparam[1],tolerance);
else
{
printf("------------------------ FC3D - NSN - no convergence after %d iterations, residual : %g < %g \n", iter, options->dparam[1], tolerance);
}
}
options->iparam[SICONOS_IPARAM_ITER_DONE] = iter;
if (!options->dWork)
{
assert(buffer);
free(buffer);
options->dWork = NULL;
}
else
{
assert(buffer == options->dWork);
}
if (!options->dWork)
{
freeNumericsMatrix(AWpB);
freeNumericsMatrix(AWpB_backup);
free(AWpB);
free(AWpB_backup);
}
if (verbose > 0)
printf("------------------------ FC3D - NSN - End\n");
}
int frictionContactFBLSA(
fc3d_nonsmooth_Newton_solvers* equation,
double *reaction,
double *velocity,
double *mu,
double *rho,
double *F,
double *A,
double *B,
NumericsMatrix *W,
double *qfree,
NumericsMatrix *blockAWpB,
double *direction,
double *tmp,
double alpha[1],
unsigned int maxiter_ls)
{
unsigned problemSize = W->size0;
// notes :
// - F contains FB or grad FB merit
// - tmp contains direction, scal*direction, reaction+scal*direction
// cf Newton_Methods.c, L59
double p = 2.1;
double fblsa_rho = 1e-8;
double gamma = 1e-4;
double scal = 1.;
// F <- compute fb
fc3d_FischerBurmeisterFunction(problemSize,
(FischerBurmeisterFun3x3Ptr) fc3d_FischerBurmeisterFunctionGenerated,
reaction,
velocity,
mu,
rho,
F,
NULL,
NULL);
double thetafb0 = 0.5 * cblas_ddot(problemSize, F, 1, F, 1);
// F <- compute gradient of fb merit function (ugly)
fc3d_FischerBurmeisterFunction(problemSize,
(FischerBurmeisterFun3x3Ptr) fc3d_FischerBurmeisterGradMeritFunctionGenerated,
reaction,
velocity,
mu,
rho,
F,
NULL,
NULL);
double norm_dir_exp_p = pow(cblas_dnrm2(problemSize, direction, 1), p);
double gradmeritfb_dir = cblas_ddot(problemSize, F, 1, direction, 1);
if (!isnan(gradmeritfb_dir) && !isinf(gradmeritfb_dir) && gradmeritfb_dir > (-fblsa_rho * norm_dir_exp_p))
{
if (verbose > 0)
{
printf("fc3d FBLSA: condition 9.1.6 unsatisfied, gradmeritfb_dir=%g, norm_r=%g\n", gradmeritfb_dir, norm_dir_exp_p);
}
// FIX: failure...
if (verbose > 0)
{
printf("fc3d FBLSA: set d^k to - grad merit(fb)\n");
}
cblas_dcopy(problemSize, F, 1, direction, 1);
cblas_dscal(problemSize, -1, direction, 1);
}
for (unsigned int iter = 0; iter < maxiter_ls; ++iter)
{
scal /= 2.;
// tmp <- 2^(-ik)*direction+reaction
cblas_dcopy(problemSize, reaction, 1, tmp, 1);
cblas_daxpy(problemSize, scal, direction, 1, tmp, 1);
// velocity <- W*tmp + qfree
cblas_dcopy(problemSize, qfree, 1, velocity, 1);
NM_gemv(1., W, tmp, 1., velocity);
// compute fb
fc3d_FischerBurmeisterFunction(problemSize,
(FischerBurmeisterFun3x3Ptr) fc3d_FischerBurmeisterFunctionGenerated,
tmp,
velocity,
mu,
rho,
F,
NULL,
NULL);
double thetafb = 0.5 * cblas_ddot(problemSize, F, 1, F, 1);
// compute grad merit fb (ugly)
fc3d_FischerBurmeisterFunction(problemSize,
(FischerBurmeisterFun3x3Ptr) fc3d_FischerBurmeisterGradMeritFunctionGenerated,
tmp,
velocity,
mu,
rho,
F,
NULL,
NULL);
// tmp <- scal*direction
cblas_dscal(problemSize, 0., tmp, 1);
cblas_daxpy(problemSize, scal, direction, 1, tmp, 1);
double grad_meritf_reaction = cblas_ddot(problemSize, F, 1, tmp, 1);
if (!isinf(grad_meritf_reaction) && !isnan(grad_meritf_reaction) &&
thetafb < thetafb0 + gamma * scal * grad_meritf_reaction)
{
if (verbose > 0)
{
printf("fc3d FBLSA success. iteration = %i, thetafb=%g, thetafb0=%g, gradmeritf,reaction=%g\n", iter, thetafb, thetafb0, gamma*scal*grad_meritf_reaction);
}
// tmp <- reaction + tmp
cblas_daxpy(problemSize, 1, reaction, 1, tmp, 1);
return 0;
}
}
if (verbose > 0)
{
printf("fc3d FBLSA reached the max number of iteration reached = %i\n", maxiter_ls);
}
return -1;
}
int globalLineSearchGP(
fc3d_nonsmooth_Newton_solvers* equation,
double *reaction,
double *velocity,
double *mu,
double *rho,
double *F,
double *A,
double *B,
NumericsMatrix *W,
double *qfree,
NumericsMatrix *AWpB,
double *direction,
double *tmp,
double alpha[1],
unsigned int maxiter_ls)
{
unsigned problemSize = W->size0;
double inf = 1e10;
double alphamin = 0.0;
double alphamax = inf;
double m1 = 0.01, m2 = 0.99;
// Computation of q(t) and q'(t) for t =0
double q0 = 0.5 * cblas_ddot(problemSize, F, 1, F, 1);
if (isnan(q0) || isinf(q0))
{
if (verbose > 0)
{
fprintf(stderr, "global line search warning. q0 is not a finite number.\n");
}
return -1;
}
// tmp <- AWpB * direction
NM_gemv(1., AWpB, direction, 0., tmp);
double dqdt0 = cblas_ddot(problemSize, F, 1, tmp, 1);
for (unsigned int iter = 0; iter < maxiter_ls; ++iter)
{
// tmp <- alpha*direction+reaction
cblas_dcopy(problemSize, reaction, 1, tmp, 1);
cblas_daxpy(problemSize, alpha[0], direction, 1, tmp, 1);
// velocity <- W*tmp + qfree
cblas_dcopy(problemSize, qfree, 1, velocity, 1);
NM_gemv(1., W, tmp, 1., velocity);
equation->function(equation->data, problemSize, tmp,
velocity, mu, rho, F,
NULL, NULL);
double q = 0.5 * cblas_ddot(problemSize, F, 1, F, 1);
if (isnan(q) || isinf(q))
{
printf("global line search warning. q is not a finite number.\n");
return -1;
}
assert(q >= 0);
double slope = (q - q0) / alpha[0];
int C1 = (slope >= m2 * dqdt0);
int C2 = (slope <= m1 * dqdt0);
if (C1 && C2)
{
if (verbose > 0)
{
printf(" globalLineSearchGP. success. ls_iter = %i alpha = %.10e, q = %.10e\n", iter, alpha[0], q);
}
return 0;
}
else if (!C1)
{
alphamin = alpha[0];
}
else
{
// not(C2)
alphamax = alpha[0];
}
if (alpha[0] < inf)
{
alpha[0] = 0.5 * (alphamin + alphamax);
}
else
{
alpha[0] = alphamin;
}
}
if (verbose > 0)
{
printf("global line search reached the max number of iteration = %i with alpha = %.10e \n", maxiter_ls, alpha[0]);
}
return -1;
}
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->tau;
int n = problem->n;
cblas_dcopy(n , problem->q , incx , w , incy); // w <-q
// Compute the current velocity
NM_gemv(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=0;
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 norm_q = cblas_dnrm2(n , problem->q , incx);
DEBUG_PRINTF("norm_q = %12.8e\n", norm_q);
*error = *error / (norm_q + 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;
}
/* Alart & Curnier solver for sparse global problem */
void gfc3d_nonsmooth_Newton_AlartCurnier(
GlobalFrictionContactProblem* problem,
double *reaction,
double *velocity,
double *globalVelocity,
int *info,
SolverOptions* options)
{
assert(problem);
assert(reaction);
assert(velocity);
assert(info);
assert(options);
assert(problem->dimension == 3);
assert(options->iparam);
assert(options->dparam);
assert(problem->q);
assert(problem->mu);
assert(problem->M);
assert(problem->H);
assert(!problem->M->matrix0);
// assert(problem->M->matrix1);
assert(!options->iparam[4]); // only host
/* M is square */
assert(problem->M->size0 == problem->M->size1);
assert(problem->M->size0 == problem->H->size0);
unsigned int iter = 0;
unsigned int itermax = options->iparam[0];
unsigned int erritermax = options->iparam[7];
if (erritermax == 0)
{
/* output a warning here */
erritermax = 1;
}
assert(itermax > 0);
assert(options->iparam[3] > 0);
double tolerance = options->dparam[0];
assert(tolerance > 0);
if (verbose > 0)
printf("------------------------ GFC3D - _nonsmooth_Newton_AlartCurnier - Start with tolerance = %g\n", tolerance);
/* sparse triplet storage */
NM_triplet(problem->M);
NM_triplet(problem->H);
unsigned int ACProblemSize = sizeOfPsi(NM_triplet(problem->M),
NM_triplet(problem->H));
unsigned int globalProblemSize = (unsigned)NM_triplet(problem->M)->m;
unsigned int localProblemSize = problem->H->size1;
assert((int)localProblemSize == problem->numberOfContacts * problem->dimension);
assert((int)globalProblemSize == problem->H->size0); /* size(velocity) ==
* Htrans*globalVelocity */
AlartCurnierFun3x3Ptr computeACFun3x3 = NULL;
switch (options->iparam[10])
{
case 0:
{
computeACFun3x3 = &computeAlartCurnierSTD;
break;
}
case 1:
{
computeACFun3x3 = &computeAlartCurnierJeanMoreau;
break;
};
case 2:
{
computeACFun3x3 = &fc3d_AlartCurnierFunctionGenerated;
break;
}
case 3:
{
computeACFun3x3 = &fc3d_AlartCurnierJeanMoreauFunctionGenerated;
break;
}
}
if(options->iparam[9] == 0)
{
/* allocate memory */
assert(options->dWork == NULL);
assert(options->iWork == NULL);
options->dWork = (double *) malloc(
(localProblemSize + /* F */
3 * localProblemSize + /* A */
3 * localProblemSize + /* B */
localProblemSize + /* rho */
ACProblemSize + /* psi */
ACProblemSize + /* rhs */
ACProblemSize + /* tmp2 */
ACProblemSize + /* tmp3 */
ACProblemSize /* solution */) * sizeof(double));
/* XXX big hack here */
options->iWork = (int *) malloc(
(3 * localProblemSize + /* iA */
3 * localProblemSize + /* iB */
3 * localProblemSize + /* pA */
3 * localProblemSize) /* pB */
* sizeof(csi));
options->iparam[9] = 1;
}
assert(options->dWork != NULL);
assert(options->iWork != NULL);
double *F = options->dWork;
double *A = F + localProblemSize;
double *B = A + 3 * localProblemSize;
double *rho = B + 3 * localProblemSize;
double * psi = rho + localProblemSize;
double * rhs = psi + ACProblemSize;
double * tmp2 = rhs + ACProblemSize;
double * tmp3 = tmp2 + ACProblemSize;
double * solution = tmp3 + ACProblemSize;
/* XXX big hack --xhub*/
csi * iA = (csi *)options->iWork;
csi * iB = iA + 3 * localProblemSize;
csi * pA = iB + 3 * localProblemSize;
csi * pB = pA + 3 * localProblemSize;
CSparseMatrix A_;
CSparseMatrix B_;
CSparseMatrix *J;
A_.p = pA;
B_.p = pB;
A_.i = iA;
B_.i = iB;
init3x3DiagBlocks(problem->numberOfContacts, A, &A_);
init3x3DiagBlocks(problem->numberOfContacts, B, &B_);
J = cs_spalloc(NM_triplet(problem->M)->n + A_.m + B_.m,
NM_triplet(problem->M)->n + A_.m + B_.m,
NM_triplet(problem->M)->nzmax + 2*NM_triplet(problem->H)->nzmax +
2*A_.n + A_.nzmax + B_.nzmax, 1, 1);
assert(A_.n == problem->H->size1);
assert(A_.nz == problem->numberOfContacts * 9);
assert(B_.n == problem->H->size1);
assert(B_.nz == problem->numberOfContacts * 9);
fc3d_AlartCurnierFunction(
localProblemSize,
computeACFun3x3,
reaction, velocity,
problem->mu, rho,
F, A, B);
csi Astart = initACPsiJacobian(NM_triplet(problem->M),
NM_triplet(problem->H),
&A_, &B_, J);
assert(Astart > 0);
assert(A_.m == A_.n);
assert(B_.m == B_.n);
assert(A_.m == problem->H->size1);
// compute rho here
for(unsigned int i = 0; i < localProblemSize; ++i) rho[i] = 1.;
// direction
for(unsigned int i = 0; i < ACProblemSize; ++i) rhs[i] = 0.;
// quick hack to make things work
// need to use the functions from NumericsMatrix --xhub
NumericsMatrix *AA_work = createNumericsMatrix(NM_SPARSE, (int)J->m, (int)J->n);
NumericsSparseMatrix* SM = newNumericsSparseMatrix();
SM->triplet = J;
NumericsMatrix *AA = createNumericsMatrixFromData(NM_SPARSE, (int)J->m, (int)J->n, SM);
info[0] = 1;
/* update local velocity from global velocity */
/* an assertion ? */
cblas_dcopy(localProblemSize, problem->b, 1, velocity, 1);
NM_tgemv(1., problem->H, globalVelocity, 1, velocity);
double linear_solver_residual=0.0;
while(iter++ < itermax)
{
/* compute psi */
ACPsi(problem, computeACFun3x3, globalVelocity, reaction, velocity, rho, psi);
/* compute A & B */
fc3d_AlartCurnierFunction(localProblemSize,
computeACFun3x3,
reaction, velocity,
problem->mu, rho,
F, A, B);
/* update J */
updateACPsiJacobian(NM_triplet(problem->M),
NM_triplet(problem->H),
&A_, &B_, J, Astart);
/* rhs = -psi */
cblas_dcopy(ACProblemSize, psi, 1, rhs, 1);
cblas_dscal(ACProblemSize, -1., rhs, 1);
/* get compress column storage for linear ops */
CSparseMatrix* Jcsc = cs_compress(J);
/* Solve: J X = -psi */
/* Solve: AWpB X = -F */
NM_copy(AA, AA_work);
int info_solver = NM_gesv(AA_work, rhs);
if (info_solver > 0)
{
fprintf(stderr, "------------------------ GFC3D - NSN_AC - solver failed info = %d\n", info_solver);
break;
info[0] = 2;
CHECK_RETURN(!cs_check_triplet(NM_triplet(AA_work)));
}
/* Check the quality of the solution */
if (verbose > 0)
{
cblas_dcopy_msan(ACProblemSize, psi, 1, tmp3, 1);
NM_gemv(1., AA, rhs, 1., tmp3);
linear_solver_residual = cblas_dnrm2(ACProblemSize, tmp3, 1);
/* fprintf(stderr, "fc3d esolve: linear equation residual = %g\n", */
/* cblas_dnrm2(problemSize, tmp3, 1)); */
/* for the component wise scaled residual: cf mumps &
* http://www.netlib.org/lapack/lug/node81.html */
}
/* line search */
double alpha = 1;
/* set current solution */
for(unsigned int i = 0; i < globalProblemSize; ++i)
{
solution[i] = globalVelocity[i];
}
for(unsigned int i = 0; i < localProblemSize; ++i)
{
solution[i+globalProblemSize] = velocity[i];
solution[i+globalProblemSize+localProblemSize] = reaction[i];
}
DEBUG_EXPR_WE(
for(unsigned int i = 0; i < globalProblemSize; ++i)
{
printf("globalVelocity[%i] = %6.4e\n",i,globalVelocity[i]);
}
for(unsigned int i = 0; i < localProblemSize; ++i)
{
printf("velocity[%i] = %6.4e\t",i,velocity[i]);
printf("reaction[%i] = %6.4e\n",i,reaction[i]);
}
);
int info_ls = _globalLineSearchSparseGP(problem,
computeACFun3x3,
solution,
rhs,
globalVelocity,
reaction, velocity,
problem->mu, rho, F, psi, Jcsc,
tmp2, &alpha, 100);
cs_spfree(Jcsc);
if(!info_ls)
{
cblas_daxpy(ACProblemSize, alpha, rhs, 1, solution, 1);
}
else
{
cblas_daxpy(ACProblemSize, 1, rhs, 1., solution, 1);
}
for(unsigned int e = 0 ; e < globalProblemSize; ++e)
{
globalVelocity[e] = solution[e];
}
for(unsigned int e = 0 ; e < localProblemSize; ++e)
{
velocity[e] = solution[e+globalProblemSize];
}
for(unsigned int e = 0; e < localProblemSize; ++e)
{
reaction[e] = solution[e+globalProblemSize+localProblemSize];
}
options->dparam[1] = INFINITY;
if(!(iter % erritermax))
{
gfc3d_compute_error(problem,
reaction, velocity, globalVelocity,
tolerance,
&(options->dparam[1]));
}
if(verbose > 0)
printf("------------------------ GFC3D - NSN_AC - iteration %d, residual = %g, linear solver residual = %g, tolerance = %g \n", iter, options->dparam[1],linear_solver_residual, tolerance);
if(options->dparam[1] < tolerance)
{
info[0] = 0;
break;
}
}
