int LocalNonsmoothNewtonSolver(FrictionContactProblem* localproblem, double * R, int *iparam, double *dparam)
{
double mu = localproblem->mu[0];
double * qLocal = localproblem->q;
double * MLocal = localproblem->M->matrix0;
double Tol = dparam[0];
double itermax = iparam[0];
int i, j, k, inew;
// store the increment
double dR[3] = {0., 0., 0.};
// store the value fo the function
double F[3] = {0., 0., 0.};
// Store the (sub)-gradient of the function
double A[9] = {0., 0., 0., 0., 0., 0., 0., 0., 0.};
double B[9] = {0., 0., 0., 0., 0., 0., 0., 0., 0.};
// Value of AW+B
double AWplusB[9] = {0., 0., 0., 0., 0., 0., 0., 0., 0.};
// Compute values of Rho (should be here ?)
double rho[3] = {1., 1., 1.};
#ifdef OPTI_RHO
computerho(localproblem, rho);
#endif
// compute the velocity
double velocity[3] = {0., 0., 0.};
for (i = 0; i < 3; i++) velocity[i] = MLocal[i + 0 * 3] * R[0] + qLocal[i]
+ MLocal[i + 1 * 3] * R[1] +
+ MLocal[i + 2 * 3] * R[2] ;
for (inew = 0 ; inew < itermax ; ++inew) // Newton iteration
{
//Update function and gradient
Function(R, velocity, mu, rho, F, A, B);
#ifndef AC_Generated
#ifndef NDEBUG
double Fg[3] = {0., 0., 0.};
double Ag[9] = {0., 0., 0., 0., 0., 0., 0., 0., 0.};
double Bg[9] = {0., 0., 0., 0., 0., 0., 0., 0., 0.};
double AWpB[9];
assert(*rho > 0. && *(rho + 1) > 0. && *(rho + 2) > 0.);
#ifdef AC_STD
frictionContact3D_AlartCurnierFunctionGenerated(R, velocity, mu, rho, Fg, Ag, Bg);
#endif
#ifdef AC_JeanMoreau
frictionContact3D_AlartCurnierJeanMoreauFunctionGenerated(R, velocity, mu, rho, Fg, Ag, Bg);
#endif
sub3(F, Fg);
sub3x3(A, Ag);
sub3x3(B, Bg);
assert(hypot3(Fg) <= 1e-7);
assert(hypot9(Ag) <= 1e-7);
assert(hypot9(Bg) <= 1e-7);
cpy3x3(A, Ag);
cpy3x3(B, Bg);
mm3x3(A, MLocal, AWpB);
add3x3(B, AWpB);
#endif
#endif
// compute -(A MLocal +B)
for (i = 0; i < 3; i++)
{
for (j = 0; j < 3; j++)
{
AWplusB[i + 3 * j] = 0.0;
for (k = 0; k < 3; k++)
{
AWplusB[i + 3 * j] -= A[i + 3 * k] * MLocal[k + j * 3];
}
AWplusB[i + 3 * j] -= B[i + 3 * j];
}
}
#ifdef AC_STD
#ifndef NDEBUG
scal3x3(-1., AWpB);
sub3x3(AWplusB, AWpB);
assert(hypot9(AWpB) <= 1e-7);
#endif
#endif
// Solve the linear system
solv3x3(AWplusB, dR, F);
// upate iterates
R[0] += dR[0];
R[1] += dR[1];
R[2] += dR[2];
// compute new residue
for (i = 0; i < 3; i++) velocity[i] = MLocal[i + 0 * 3] * R[0] + qLocal[i]
+ MLocal[i + 1 * 3] * R[1] +
+ MLocal[i + 2 * 3] * R[2] ;
Function(R, velocity, mu, rho, F, NULL, NULL);
dparam[1] = 0.5 * (F[0] * F[0] + F[1] * F[1] + F[2] * F[2]) / (1.0 + sqrt(R[0] * R[0] + R[1] * R[1] + R[2] * R[2])) ; // improve with relative tolerance
/* dparam[2] =0.0;
FrictionContact3D_unitary_compute_and_add_error( R , velocity,mu, &(dparam[2]));*/
if (verbose > 1) printf("----------------------------------- LocalNewtonSolver number of iteration = %i error = %.10e \n", inew, dparam[1]);
if (dparam[1] < Tol)
{
/* printf("----------------------------------- LocalNewtonSolver number of iteration = %i error = %.10e \t error2 = %.10e \n",inew,dparam[1], dparam[2]); */
return 0;
}
}// End of the Newton iteration
/* printf("----------------------------------- LocalNewtonSolver number of iteration = %i error = %.10e \t error2 = %.10e \n",inew,dparam[1], dparam[2]); */
return 1;
}
static void fc3d_AC_initialize(FrictionContactProblem* problem,
FrictionContactProblem* localproblem,
SolverOptions * options)
{
/** In initialize, these operators are "connected" to their corresponding static variables,
* that will be used to build local problem for each considered contact.
* Local problem is built during call to update (which depends on the storage type for M).
*/
DEBUG_PRINTF("fc3d_AC_initialize starts with options->iparam[10] = %i\n",
options->iparam[SICONOS_FRICTION_3D_NSN_FORMULATION]);
if (options->iparam[SICONOS_FRICTION_3D_NSN_FORMULATION] ==
SICONOS_FRICTION_3D_NSN_FORMULATION_ALARTCURNIER_STD )
{
Function = &(computeAlartCurnierSTD);
}
else if (options->iparam[SICONOS_FRICTION_3D_NSN_FORMULATION] ==
SICONOS_FRICTION_3D_NSN_FORMULATION_JEANMOREAU_STD )
{
Function = &(computeAlartCurnierJeanMoreau);
}
else if (options->iparam[SICONOS_FRICTION_3D_NSN_FORMULATION] ==
SICONOS_FRICTION_3D_NSN_FORMULATION_ALARTCURNIER_GENERATED )
{
Function = &(fc3d_AlartCurnierFunctionGenerated);
}
else if (options->iparam[SICONOS_FRICTION_3D_NSN_FORMULATION] ==
SICONOS_FRICTION_3D_NSN_FORMULATION_JEANMOREAU_GENERATED )
{
Function = &fc3d_AlartCurnierJeanMoreauFunctionGenerated;;
}
else if (options->iparam[SICONOS_FRICTION_3D_NSN_FORMULATION] ==
SICONOS_FRICTION_3D_NSN_FORMULATION_NULL)
{
Function = NULL;
}
/* Compute and store default value of rho value */
int nc = problem->numberOfContacts;
double avg_rho[3] = {0.0, 0.0, 0.0};
if (options->solverId == SICONOS_FRICTION_3D_ONECONTACT_NSN ||
options->solverId == SICONOS_FRICTION_3D_ONECONTACT_NSN_GP)
{
if (!options->dWork ||
options->dWorkSize < 3*nc)
{
options->dWork = (double *)realloc(options->dWork,
3*nc * sizeof(double));
options->dWorkSize = 3*nc ;
}
}
else if (options->solverId == SICONOS_FRICTION_3D_ONECONTACT_NSN_GP_HYBRID)
{
if (!options->dWork ||
options->dWorkSize < 4*nc)
{
options->dWork = (double *)realloc(options->dWork,
4*nc * sizeof(double));
options->dWorkSize = 4*nc ;
}
}
double * rho;
for (int contact =0; contact <nc ; contact++)
{
if (options->solverId == SICONOS_FRICTION_3D_ONECONTACT_NSN ||
options->solverId == SICONOS_FRICTION_3D_ONECONTACT_NSN_GP)
{
rho = &options->dWork[3*contact];
}
else if (options->solverId == SICONOS_FRICTION_3D_ONECONTACT_NSN_GP_HYBRID)
{
options->dWork[contact] = 1.0; // for PLI algorithm.
rho = &options->dWork[3*contact+nc];
}
numerics_printf("fc3d_AC_initialize "" compute rho for contact = %i",contact);
if (options->iparam[SICONOS_FRICTION_3D_NSN_RHO_STRATEGY] == SICONOS_FRICTION_3D_NSN_FORMULATION_RHO_STRATEGY_SPLIT_SPECTRAL_NORM_COND)
{
fc3d_local_problem_fill_M(problem, localproblem, contact);
compute_rho_split_spectral_norm_cond(localproblem, rho);
}
else if (options->iparam[SICONOS_FRICTION_3D_NSN_RHO_STRATEGY] == SICONOS_FRICTION_3D_NSN_FORMULATION_RHO_STRATEGY_SPLIT_SPECTRAL_NORM)
{
fc3d_local_problem_fill_M(problem, localproblem, contact);
compute_rho_split_spectral_norm(localproblem, rho);
}
else if (options->iparam[SICONOS_FRICTION_3D_NSN_RHO_STRATEGY] == SICONOS_FRICTION_3D_NSN_FORMULATION_RHO_STRATEGY_SPECTRAL_NORM)
{
fc3d_local_problem_fill_M(problem, localproblem, contact);
compute_rho_spectral_norm(localproblem, rho);
}
else if (options->iparam[SICONOS_FRICTION_3D_NSN_RHO_STRATEGY] == SICONOS_FRICTION_3D_NSN_FORMULATION_RHO_STRATEGY_CONSTANT)
{
rho[0]=options->dparam[SICONOS_FRICTION_3D_NSN_RHO];
rho[1]=options->dparam[SICONOS_FRICTION_3D_NSN_RHO];
rho[2]=options->dparam[SICONOS_FRICTION_3D_NSN_RHO];
}
else if (options->iparam[SICONOS_FRICTION_3D_NSN_RHO_STRATEGY] == SICONOS_FRICTION_3D_NSN_FORMULATION_RHO_STRATEGY_ADAPTIVE)
{
numerics_error("fc3d_AC_initialize", "Adaptive strategy for computing rho not yet implemented");
}
else
numerics_error("fc3d_AC_initialize", "unknown strategy for computing rho");
if (verbose >0)
{
avg_rho[0] += rho[0];
avg_rho[1] += rho[1];
avg_rho[2] += rho[2];
}
numerics_printf("fc3d_AC_initialize""contact = %i, rho[0] = %4.2e, rho[1] = %4.2e, rho[2] = %4.2e", contact, rho[0], rho[1], rho[2]);
fc3d_local_problem_fill_M(problem, localproblem, contact);
double m_row_norm = 0.0, sum;
for (int i =0; i<3; i++ )
{
sum =0.0;
for (int j =0; j<3; j++ )
{
sum += fabs(localproblem->M->matrix0[i+j*3]);
}
m_row_norm = max(sum, m_row_norm);
}
numerics_printf("fc3d_AC_initialize" " inverse of norm of M = %e", 1.0/hypot9(localproblem->M->matrix0) );
numerics_printf("fc3d_AC_initialize" " inverse of row norm of M = %e", 1.0/m_row_norm );
DEBUG_EXPR(NM_display(localproblem->M););
}
