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; } } }
void computeDenseAWpB( double *A, NumericsMatrix *W, double *B, NumericsMatrix *AWpB) { unsigned problemSize = W->size0; double* result = AWpB->matrix0; double* Wx = W->matrix0; assert(problemSize >= 3); double Wij[9], Ai[9], Bi[9], tmp[9]; for (unsigned int ip3 = 0, ip9 = 0; ip3 < problemSize; ip3 += 3, ip9 += 9) { assert(ip9 < 3 * problemSize - 8); extract3x3(3, ip9, 0, A, Ai); extract3x3(3, ip9, 0, B, Bi); for (unsigned int jp3 = 0; jp3 < problemSize; jp3 += 3) { assert(jp3 < problemSize - 2); assert(ip3 < problemSize - 2); extract3x3(problemSize, ip3, jp3, Wx, Wij); mm3x3(Ai, Wij, tmp); if (jp3 == ip3) add3x3(Bi, tmp); insert3x3(problemSize, ip3, jp3, result, tmp); } } }
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; }