void NM_clearSparseBlock(NumericsMatrix* A)
{
if (A->matrix1)
{
freeSBM(A->matrix1);
free(A->matrix1);
A->matrix1 = NULL;
}
}
void freeNumericsMatrix(NumericsMatrix* m)
{
assert(m && "freeNumericsMatrix, m == NULL");
if (m->storageType == 0)
{
if (m->matrix0)
{
free(m->matrix0);
m->matrix0 = NULL;
}
}
else
{
if (m->matrix1)
{
freeSBM(m->matrix1);
free(m->matrix1);
m->matrix1 = NULL;
}
if (m->matrix2)
{
freeNumericsSparseMatrix(m->matrix2);
free(m->matrix2);
m->matrix2 = NULL;
}
}
if (m->internalData)
{
if (m->internalData->iWork)
{
assert (m->internalData->iWorkSize > 0);
free(m->internalData->iWork);
m->internalData->iWork = NULL;
}
free(m->internalData);
m->internalData = NULL;
}
}
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);
}
