void prodNumericsMatrix(int sizeX, int sizeY, double alpha, NumericsMatrix* A, const double* const x, double beta, double* y) { assert(A); assert(x); assert(y); assert(A->size0 == sizeY); assert(A->size1 == sizeX); int storage = A->storageType; /* double* storage */ switch (storage) { case NM_DENSE: cblas_dgemv(CblasColMajor, CblasNoTrans, sizeY, sizeX, alpha, A->matrix0, sizeY, x, 1, beta, y, 1); break; /* SparseBlock storage */ case NM_SPARSE_BLOCK: prodSBM(sizeX, sizeY, alpha, A->matrix1, x, beta, y); break; /* coordinate */ case NM_SPARSE: cs_aaxpy(alpha, NM_csc(A), x, beta, y); break; default: fprintf(stderr, "Numerics, NumericsMatrix, product matrix - vector prod(A,x,y) failed, unknown storage type for A.\n"); exit(EXIT_FAILURE); } }
/* Numerics Matrix wrapper for y <- alpha A x + beta y */ void NM_gemv(const double alpha, NumericsMatrix* A, const double *x, const double beta, double *y) { switch (A->storageType) { case NM_DENSE: { cblas_dgemv(CblasColMajor, CblasNoTrans, A->size0, A->size1, alpha, A->matrix0, A->size0, x, 1, beta, y, 1); break; } case NM_SPARSE_BLOCK: { prodSBM(A->size1, A->size0, alpha, A->matrix1, x, beta, y); break; } default: { assert(A->storageType == NM_SPARSE); CHECK_RETURN(cs_aaxpy(alpha, NM_csc(A), x, beta, y)); } } }
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); }