void frictionContact3D_AlartCurnierFunctionGenerated(
double *reaction,
double *velocity,
double mu,
double *rho,
double *f,
double *A,
double *B)
{
double result[21];
assert(reaction);
assert(velocity);
assert(rho);
SET3(reaction);
SET3(velocity);
SET3(rho);
if (f && A && B)
{
frictionContact3D_AlartCurnierFABGenerated(
*reaction0, *reaction1, *reaction2,
*velocity0, *velocity1, *velocity2,
mu,
*rho0, *rho1, *rho2,
result);
cpy3(result, f);
cpy3x3(result + 3, A);
cpy3x3(result + 12, B);
}
else
{
if (f)
{
frictionContact3D_AlartCurnierFGenerated(
*reaction0, *reaction1, *reaction2,
*velocity0, *velocity1, *velocity2,
mu,
*rho0, *rho1, *rho2,
result);
cpy3(result, f);
}
if (A && B)
{
frictionContact3D_AlartCurnierABGenerated(
*reaction0, *reaction1, *reaction2,
*velocity0, *velocity1, *velocity2,
mu,
*rho0, *rho1, *rho2,
result);
cpy3x3(result, A);
cpy3x3(result + 9, B);
}
}
}
void computeSparseAWpB(
double *A,
NumericsMatrix *W,
double *B,
NumericsMatrix *AWpB)
{
/* unsigned int problemSize = W->size0; */
SparseBlockStructuredMatrix* Wb = W->matrix1;
assert((unsigned)W->size0 >= 3);
assert((unsigned)W->size0 / 3 >= Wb->filled1 - 1);
double Ai[9], Bi[9], tmp[9];
for (unsigned int row = 0, ip9 = 0, i0 = 0;
row < Wb->filled1 - 1; ++row, ip9 += 9, i0 += 3)
{
assert(ip9 < 3 * (unsigned)W->size0 - 8);
extract3x3(3, ip9, 0, A, Ai);
extract3x3(3, ip9, 0, B, Bi);
for (unsigned int blockn = (unsigned int) Wb->index1_data[row];
blockn < Wb->index1_data[row + 1]; ++blockn)
{
unsigned int col = (unsigned int) Wb->index2_data[blockn];
mm3x3(Ai, Wb->block[blockn], tmp);
if (col == row) add3x3(Bi, tmp);
cpy3x3(tmp, AWpB->matrix1->block[blockn]);
}
}
/* Invalidation of sparse storage, if any. */
if (AWpB->matrix2)
{
if (AWpB->matrix2->triplet)
{
cs_spfree(AWpB->matrix2->triplet);
AWpB->matrix2->triplet = NULL;
}
if (AWpB->matrix2->csc)
{
cs_spfree(AWpB->matrix2->csc);
AWpB->matrix2->csc = NULL;
}
if (AWpB->matrix2->trans_csc)
{
cs_spfree(AWpB->matrix2->trans_csc);
AWpB->matrix2->trans_csc = NULL;
}
}
}
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;
}
