int main(void)
{
NumericsOptions NO;
setDefaultNumericsOptions(&NO);
NO.verboseMode = 1; // turn verbose mode to off by default
int total_info = 0;
double q[] = { -1, 1, 3, -1, 1, 3, -1, 1, 3};
double mu[] = {0.1, 0.1, 0.1};
double Wdata[81] = {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1};
NumericsMatrix* tmpM = createNumericsMatrixFromData(NM_DENSE, 9, 9, Wdata);
NumericsMatrix* W = createNumericsMatrix(NM_SPARSE, 9, 9);
NM_copy_to_sparse(tmpM, W);
int solvers_to_test[] = {SICONOS_FRICTION_3D_NSGS,
SICONOS_FRICTION_3D_NSN_AC,
SICONOS_FRICTION_3D_NSN_FB,
SICONOS_FRICTION_3D_NSN_NM,
SICONOS_FRICTION_3D_SOCLCP,
SICONOS_FRICTION_3D_PROX};
for (size_t s = 0; s < sizeof(solvers_to_test); ++s)
{
int solver_id = solvers_to_test[s];
FrictionContactProblem* FC = frictionContactProblem_new(3, 3, W, q, mu);
double r[9] = {0.};
double u[9] = {0.};
SolverOptions SO;;
fc3d_setDefaultSolverOptions(&SO, solver_id);
int info = fc3d_driver(FC, r, u, &SO, &NO);
if (info)
{
fprintf(stderr, "Solver %s failed with error %d\n", idToName(solver_id), info);
total_info = 1;
}
FC->M = NULL;
FC->q = NULL;
FC->mu = NULL;
deleteSolverOptions(&SO);
freeFrictionContactProblem(FC);
free(FC);
}
freeNumericsMatrix(W);
tmpM->matrix0 = NULL;
freeNumericsMatrix(tmpM);
free(W);
free(tmpM);
return total_info;
}
void freeSecondOrderConeLinearComplementarityProblem(SecondOrderConeLinearComplementarityProblem* problem)
{
if (problem->M)
{
freeNumericsMatrix(problem->M);
free(problem->M);
problem->M = NULL;
}
if (problem->mu)
{
free(problem->mu);
problem->mu = NULL;
}
if (problem->q)
{
free(problem->q);
problem->q = NULL;
}
if (problem->coneIndex)
{
free(problem->coneIndex);
problem->coneIndex = NULL;
}
free(problem);
}
void freeLinearComplementarityProblem(LinearComplementarityProblem* problem)
{
freeNumericsMatrix(problem->M);
free(problem->M);
free(problem->q);
free(problem);
problem = NULL;
}
void LinearSystem_freeProblem(LinearSystemProblem *problem)
{
freeNumericsMatrix(problem->M);
if (problem->M)
free(problem->M);
if (problem->q)
free(problem->q);
free(problem);
}
void freeRelay_problem(RelayProblem* problem)
{
freeNumericsMatrix(problem->M);
free(problem->M);
free(problem->q);
free(problem->lb);
free(problem->ub);
free(problem);
}
void freeGlobalFrictionContactProblem(GlobalFrictionContactProblem* problem)
{
if (problem->M)
{
freeNumericsMatrix(problem->M);
free(problem->M);
problem->M = NULL;
}
if (problem->H)
{
freeNumericsMatrix(problem->H);
free(problem->H);
problem->H = NULL;
}
if (problem->mu)
{
free(problem->mu);
problem->mu = NULL;
}
if (problem->q)
{
free(problem->q);
problem->q = NULL;
}
if (problem->b)
{
free(problem->b);
problem->b = NULL;
}
if (problem->env) assert(0 && "freeGlobalFrictionContactProblem :: problem->env != NULL, don't know what to do");
gfc3d_free_workspace(problem);
free(problem);
}
int main(void)
{
printf("========= Starts Numerics tests for NumericsMatrix ========= \n");
int i, nmm = 4 ;
NumericsMatrix ** NMM = (NumericsMatrix **)malloc(nmm * sizeof(NumericsMatrix *)) ;
for (i = 0 ; i < nmm; i++)
{
NMM[i] = newNumericsMatrix();
}
int info = test_BuildNumericsMatrix(NMM);
if (info != 0)
{
printf("Construction failed ...\n");
return info;
}
printf("Construction ok ...\n");
info = test_NM_row_prod_no_diag_non_square(NMM[2], NMM[3]);
printf("End of Sub-Prod no diag Non Square...\n");
if (info != 0) return info;
/* free memory */
for (i = 0 ; i < nmm; i++)
{
freeNumericsMatrix(NMM[i]);
free(NMM[i]);
}
free(NMM);
printf("========= End Numerics tests for NumericsMatrix ========= \n");
return info;
}
void gfc3d_free_workspace(GlobalFrictionContactProblem* problem)
{
if (problem->workspace)
{
if (problem->workspace->factorized_M)
{
freeNumericsMatrix(problem->workspace->factorized_M);
free(problem->workspace->factorized_M);
problem->workspace->factorized_M = NULL;
}
if (problem->workspace->globalVelocity)
{
free(problem->workspace->globalVelocity);
problem->workspace->globalVelocity = NULL;
}
free(problem->workspace);
problem->workspace = NULL;
}
}
void freeMixedLinearComplementarityProblem(MixedLinearComplementarityProblem* problem)
{
if (problem->isStorageType1)
{
freeNumericsMatrix(problem->M);
free(problem->M);
free(problem->q);
free(problem->blocksRows);
free(problem->blocksIsComp);
}
if (problem->isStorageType2)
{
free(problem->A);
free(problem->B);
free(problem->C);
free(problem->D);
free(problem->a);
free(problem->b);
}
free(problem);
}
int gfc3d_LmgcDriver(double *reaction,
double *velocity,
double *globalVelocity,
double *q,
double *b,
double *mu,
double *Mdata,
unsigned int nzM,
unsigned int *rowM,
unsigned int *colM,
double* Hdata,
unsigned int nzH,
unsigned int *rowH,
unsigned int *colH,
unsigned int n,
unsigned int nc,
int solver_id,
int isize,
int *iparam,
int dsize,
double *dparam,
int verbose,
int outputFile,
int freq_output)
{
/* NumericsMatrix M, H; */
NumericsMatrix * M =newNumericsMatrix();
M->storageType = 2; /* sparse */
M->size0 = n;
M->size1 = n;
NumericsMatrix * H =newNumericsMatrix();
H->storageType = 2;
H->size0 = M->size0;
H->size1 = 3 * nc;
NumericsSparseMatrix * SM =newNumericsSparseMatrix();
M->matrix2 = SM;
SM->triplet = (CSparseMatrix * )malloc(sizeof(CSparseMatrix));
CSparseMatrix * _M = SM->triplet;
SM->origin = NS_TRIPLET;
csi * _colM = alloc_memory_csi(nzM, colM);
csi * _rowM = alloc_memory_csi(nzM, rowM);
_M->nzmax = nzM;
_M->nz = nzM;
_M->m = M->size0;
_M->n = M->size1;
_M->p = (csi *) _colM;
_M->i = (csi *) _rowM;
double * _Mdata = alloc_memory_double(nzM, Mdata);
_M->x = _Mdata;
DEBUG_PRINTF("_M->n=%li\t",_M->n);
DEBUG_PRINTF("_M->m=%li\n",_M->m);
NumericsSparseMatrix * SH =newNumericsSparseMatrix();
H->matrix2 = SH;
SH->triplet = (CSparseMatrix * )malloc(sizeof(CSparseMatrix));
CSparseMatrix * _H = SH->triplet;
SH->origin = NS_TRIPLET;
csi * _colH = alloc_memory_csi(nzH, colH);
csi * _rowH = alloc_memory_csi(nzH, rowH);
_H->nzmax = nzH;
_H->nz = nzH;
_H->m = H->size0;
_H->n = H->size1;
_H->p = _colH;
_H->i = _rowH;
double * _Hdata = alloc_memory_double(nzH, Hdata);
_H->x = _Hdata;
for (int i=0; i< _M->nz; ++i)
{
_M->p[i] --;
_M->i[i] --;
/* DEBUG_PRINTF("%d -> %d,%d\n", i, _M->p[i], _M->i[i]); */
}
for (int i=0; i< _H->nz; ++i)
{
_H->p[i] --;
_H->i[i] --;
/* DEBUG_PRINTF("%d -> %d,%d\n", i, _H->p[i], _H->i[i]); */
}
GlobalFrictionContactProblem * problem =(GlobalFrictionContactProblem*)malloc(sizeof(GlobalFrictionContactProblem));
problem->dimension = 3;
problem->numberOfContacts = nc;
problem->env = NULL;
problem->workspace = NULL;
problem->M = M;
problem->H = H;
problem->q = q;
problem->b = b;
problem->mu = mu;
SolverOptions numerics_solver_options;
gfc3d_setDefaultSolverOptions(&numerics_solver_options, solver_id);
int iSize_min = isize < numerics_solver_options.iSize ? isize : numerics_solver_options.iSize;
for (int i = 0; i < iSize_min; ++i)
numerics_solver_options.iparam[i] = iparam[i];
int dSize_min = dsize < numerics_solver_options.dSize ? dsize : numerics_solver_options.dSize;
for (int i=0; i < dSize_min; ++i)
numerics_solver_options.dparam[i] = dparam[i];
/* solver_options_print(&numerics_solver_options); */
/* FILE * file = fopen("toto.dat", "w"); */
/* globalFrictionContact_printInFile(problem, file); */
/* fclose(file); */
int rinfo = gfc3d_driver(problem,
reaction,
velocity,
globalVelocity,
&numerics_solver_options);
/* FILE * file1 = fopen("tutu.dat", "w"); */
/* globalFrictionContact_printInFile(problem, file1); */
/* fclose(file1); */
if(outputFile == 1)
{
/* dump in C format */
}
else if (outputFile == 2)
{
/* dump in Numerics .dat format */
}
else if (outputFile == 3)
{
#ifdef WITH_FCLIB
fccounter++;
if (fccounter % freq_output == 0)
{
char fname[256];
snprintf(fname, sizeof(fname), "LMGC_GFC3D-i%.5d-%i-%.5d.hdf5", numerics_solver_options.iparam[7], nc, fccounter);
printf("Dump LMGC_GFC3D-i%.5d-%i-%.5d.hdf5.\n", numerics_solver_options.iparam[7], nc, fccounter);
/* printf("ndof = %i.\n", ndof); */
FILE * foutput = fopen(fname, "w");
int n = 100;
char * title = (char *)malloc(n * sizeof(char *));
strncpy(title, "LMGC dump in hdf5", n);
char * description = (char *)malloc(n * sizeof(char *));
snprintf(description, n, "Rewriting in hdf5 through siconos of %s in FCLIB format", fname);
char * mathInfo = (char *)malloc(n * sizeof(char *));
strncpy(mathInfo, "unknown", n);
globalFrictionContact_fclib_write(problem,
title,
description,
mathInfo,
fname);
fclose(foutput);
}
#else
printf("Fclib is not available ...\n");
#endif
}
freeNumericsMatrix(M);
freeNumericsMatrix(H);
free(M);
free(H);
free(problem);
/* free(_colM); */
/* free(_colH); */
/* free(_rowM); */
/* free(_rowH); */
return rinfo;
}
int test_prodNumericsMatrixNumericsMatrix(NumericsMatrix** MM)
{
NumericsMatrix * M1 = MM[0];
NumericsMatrix * M2 = MM[1];
NumericsMatrix * M3 = MM[2];
NumericsMatrix * M4 = MM[3];
int info = -1;
printf("== Numerics tests: prodNumericsMatrixNumericsMatrix(NumericsMatrix,NumericsMatrix) == \n");
int i, j, k;
double alpha = 1.0, beta = 0.0;
double tol = 1e-12;
NumericsMatrix C;
C.storageType = 0;
C.size0 = M1->size0;
C.size1 = M1->size1;
C.matrix0 = (double *)malloc(C.size0 * C.size1 * sizeof(double));
C.matrix1 = NULL;
C.matrix2 = NULL;
C.internalData = NULL;
prodNumericsMatrixNumericsMatrix(alpha, M1, M1, beta, &C);
double * Cref = (double *)malloc(C.size0 * C.size1 * sizeof(double));
double sum;
for (i = 0; i < C.size0; i++)
{
for (j = 0; j < C.size1; j++)
{
sum = 0.0;
for (k = 0; k < M1->size1; k++)
{
sum = sum + M1->matrix0[i + k * M1->size0] * M1->matrix0[k + j * M1->size0];
}
Cref[i + j * C.size0] = sum;
}
}
double err = 0.0;
for (i = 0; i < C.size0; i++)
{
for (j = 0; j < C.size1; j++)
{
printf("Cref[%i+%i*%i]= %lf\t\t", i, j, C.size0, Cref[i + j * C.size0]);
printf("Cmatrix0[%i+%i*%i]= %lf\t", i, j, C.size0, C.matrix0[i + j * C.size0]);
err += (Cref[i + j * C.size0] - C.matrix0[i + j * C.size0]) * (Cref[i + j * C.size0] - C.matrix0[i + j * C.size0]);
printf("err = %lf\n", err);
}
}
if (err < tol)
{
info = 0;
}
if (info == 0)
printf("Step 0 ( C = alpha*A*B + beta*C, double* storage, square matrix ) ok ...\n");
else
{
printf("Step 0 ( C = alpha*A*B + beta*C, double* storage, square matrix) failed ...\n");
goto exit_1;
}
NumericsMatrix C2;
C2.storageType = 0;
C2.size0 = M1->size0;
C2.size1 = M3->size1;
C2.matrix0 = (double *)malloc(C2.size0 * C2.size1 * sizeof(double));
C2.matrix1 = NULL;
C2.matrix2 = NULL;
C2.internalData = NULL;
prodNumericsMatrixNumericsMatrix(alpha, M1, M3, beta, &C2);
double * C2ref = (double *)malloc(C2.size0 * C2.size1 * sizeof(double));
for (i = 0; i < C2.size0; i++)
{
for (j = 0; j < C2.size1; j++)
{
sum = 0.0;
for (k = 0; k < M1->size1; k++)
{
sum = sum + M1->matrix0[i + k * M1->size0] * M3->matrix0[k + j * M3->size0];
}
C2ref[i + j * C2.size0] = sum;
/* printf("C2ref(%i,%i)=%f\n", i,j,C2ref[i+j*C2.size0] ); */
}
}
err = 0.0;
for (i = 0; i < C2.size0; i++)
{
for (j = 0; j < C2.size1; j++)
{
err += (C2ref[i + j * C2.size0] - C2.matrix0[i + j * C2.size0]) * (C2ref[i + j * C2.size0] - C2.matrix0[i + j * C2.size0]);
}
}
if (err < tol)
{
info = 0;
}
if (info == 0)
printf("Step 1 ( C = alpha*A*B + beta*C, double* storage, non square) ok ...\n");
else
{
printf("Step 1 ( C = alpha*A*B + beta*C, double* storage, non square) failed ...\n");
goto exit_2;
}
NumericsMatrix C3;
C3.storageType = 1;
C3.size0 = M2->size0;
C3.size1 = M2->size1;
SparseBlockStructuredMatrix * SBM3 = (SparseBlockStructuredMatrix *)malloc(sizeof(SparseBlockStructuredMatrix));
C3.matrix1 = SBM3;
C3.matrix2 = NULL;
C3.internalData = NULL;
beta = 1.0;
i = 1;
// while (i > 0)
allocateMemoryForProdSBMSBM(M2->matrix1, M2->matrix1, SBM3);
/* printSBM(SBM3); */
prodNumericsMatrixNumericsMatrix(alpha, M2, M2, beta, &C3);
//freeSBM(SBM3);
printf("i= %i\n", i++);
/* Check if it is correct */
/* C3 and CRef must have the same values.*/
for (i = 0; i < C3.size0; i++)
{
for (j = 0; j < C3.size1; j++)
{
if (fabs(Cref[i + j * C3.size0] - getValueSBM(C3.matrix1, i, j)) > tol) info = 1;
/* printf("%i\t%i\n",i,j); */
/* printf("%lf\n",fabs(Cref[i+j*C3.size0]-getValueSBM(C3.matrix1,i,j) )); */
/* printf("%lf\n",Cref[i+j*C3.size0]); */
/* printf("%lf\n",getValueSBM(C3.matrix1,i,j)); */
if (info == 1) break;
}
if (info == 1) break ;
}
if (info == 0)
printf("Step 2 ( C = alpha*A*B + beta*C, sparse storage) ok ...\n");
else
{
printf("Step 2 ( C = alpha*A*B + beta*C, sparse storage) failed ...\n");
goto exit_3;
}
NumericsMatrix C4;
C4.storageType = 1;
C4.size0 = M2->size0;
C4.size1 = M4->size1;
SparseBlockStructuredMatrix * SBM4 = (SparseBlockStructuredMatrix *)malloc(sizeof(SparseBlockStructuredMatrix));
C4.matrix1 = SBM4;
C4.matrix2 = NULL;
C4.internalData = NULL;
allocateMemoryForProdSBMSBM(M2->matrix1, M4->matrix1, SBM4);
/* printSBM(SBM4); */
prodNumericsMatrixNumericsMatrix(alpha, M2, M4, beta, &C4);
/* Check if it is correct */
/* C4 and C2Ref must have the same values.*/
for (i = 0; i < C4.size0; i++)
{
for (j = 0; j < C4.size1; j++)
{
if (fabs(C2ref[i + j * C4.size0] - getValueSBM(C4.matrix1, i, j)) > tol) info = 1;
/* printf("%i\t%i\n",i,j); */
/* printf("%lf\n",fabs(C2ref[i+j*C4.size0]-getValueSBM(C4.matrix1,i,j) )); */
/* printf("%lf\n",C2ref[i+j*C4.size0]); */
/* printf("%lf\n",getValueSBM(C4.matrix1,i,j)); */
if (info == 1) break;
}
if (info == 1) break ;
}
if (info == 0)
printf("Step 3 ( C = alpha*A*B + beta*C, sparse storage) ok ...\n");
else
{
printf("Step 3 ( C = alpha*A*B + beta*C, sparse storage) failed ...\n");
}
freeNumericsMatrix(&C4);
exit_3:
freeNumericsMatrix(&C3);
exit_2:
free(C2.matrix0);
free(C2ref);
exit_1:
free(Cref);
free(C.matrix0);
return info;
}
void relay_avi_caoferris(RelayProblem* problem, double *z, double *w, int *info, SolverOptions* options)
{
unsigned int n = problem->size;
assert(n > 0);
unsigned int s = 2*n;
/* Copy the data from Relay problem */
LinearComplementarityProblem lcplike_pb;
lcplike_pb.size = s;
NumericsMatrix num_mat;
fillNumericsMatrix(&num_mat, NM_DENSE, s, s, calloc(s*s, sizeof(double)));
lcplike_pb.M = &num_mat;
lcplike_pb.q = (double *)malloc(s*sizeof(double));
double* b_bar = (double *)malloc(s*sizeof(double));
/* We can always choose the extreme point such that the matrix of active
* constrains is the identity and the other matrix is minus the identity.
* B_A = Id, B_I = -Id, b_A = lb, b_I = -ub
* TODO: implement the case when the user gives an hint about the solution
*
* Siconos/Kernel is giving us column-major matrix and the solver is
* expecting matrix in this format
* */
double tmp;
for (unsigned int i = 0; i < n; ++i)
{
tmp = 0.0;
lcplike_pb.M->matrix0[i*(s+1)] = 1.0;
lcplike_pb.M->matrix0[(i + n)*(s+1)] = -1.0;
lcplike_pb.q[i] = problem->q[i];
lcplike_pb.q[i+n] = - problem->lb[i] + problem->ub[i];
for (unsigned j = 0; j < n; ++j)
{
lcplike_pb.M->matrix0[i + (j+n)*s] = problem->M->matrix0[i + j*n];
tmp += problem->M->matrix0[i + j*n]*problem->lb[j];
}
/* \bar{a} = -\tilde{a} + Ay_e */
lcplike_pb.q[i] += tmp;
}
double* d_vec = (double *)malloc(s*sizeof(double));
for (unsigned i = 0; i<n; ++i)
{
d_vec[i] = -1.0;
d_vec[i+n] = 0;
}
/* Set of active constraint is trivial */
unsigned* A = (unsigned*)malloc(n*sizeof(unsigned));
for (unsigned i = 0; i < n; ++i) A[i] = i + 1;
double* u_vec = (double *)calloc(s, sizeof(double));
double* s_vec = (double *)calloc(s, sizeof(double));
/* Call directly the 3rd stage
* Here w is used as u and z as s in the AVI */
*info = avi_caoferris_stage3(&lcplike_pb, u_vec, s_vec, d_vec, n, A, options);
/* Update z */
/* XXX why no w ? */
DEBUG_PRINT_VEC_INT(A, n);
for (unsigned i = 0; i<n; ++i) z[i] = s_vec[A[i]-1] + problem->lb[i];
/* free allocated stuff */
free(u_vec);
free(s_vec);
free(A);
free(d_vec);
freeNumericsMatrix(lcplike_pb.M);
free(lcplike_pb.q);
free(b_bar);
}
int main(int argc, char* argv[])
{
// Problem Definition
int info = -1;
int NC = 1;//Number of contacts
int Ndof = 9;//Number of DOF
// Problem Definition
double M11[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; // Warning Fortran Storage
double M22[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; // Warning Fortran Storage
double M33[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; // Warning Fortran Storage
/* double M[81] = {1, 0, 0, 0, 0, 0, 0, 0, 0, */
/* 0, 1, 0, 0, 0, 0, 0, 0, 0, */
/* 0, 0, 1, 0, 0, 0, 0, 0, 0, */
/* 0, 0, 0, 1, 0, 0, 0, 0, 0, */
/* 0, 0, 0, 0, 1, 0, 0, 0, 0, */
/* 0, 0, 0, 0, 0, 1, 0, 0, 0, */
/* 0, 0, 0, 0, 0, 0, 1, 0, 0, */
/* 0, 0, 0, 0, 0, 0, 0, 1, 0, */
/* 0, 0, 0, 0, 0, 0, 0, 0, 1}; */
double H00[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
double H20[9] = { -1, 0, 0, 0, -1, 0, 0, 0, -1};
/* double H[27] = {1, 0, 0, 0, 0, 0, -1, 0, 0, */
/* 0, 1, 0, 0, 0, 0, 0, -1, 0, */
/* 0, 0, 1, 0, 0, 0, 0, 0, -1}; */
double q[9] = { -3, -3, -3, -1, 1, 3, -1, 1, 3};
double b[3] = {0, 0, 0};
double mu[1] = {0.1};
/* DSCAL(9,-1.0,q,1); */
/* int NC = 3;//Number of contacts */
/* int Ndof = 9;//Number of DOF */
/* double M[81] = {1, 0, 0, 0, 0, 0, 0, 0, 0, */
/* 0, 1, 0, 0, 0, 0, 0, 0, 0, */
/* 0, 0, 1, 0, 0, 0, 0, 0, 0, */
/* 0, 0, 0, 1, 0, 0, 0, 0, 0, */
/* 0, 0, 0, 0, 1, 0, 0, 0, 0, */
/* 0, 0, 0, 0, 0, 1, 0, 0, 0, */
/* 0, 0, 0, 0, 0, 0, 1, 0, 0, */
/* 0, 0, 0, 0, 0, 0, 0, 1, 0, */
/* 0, 0, 0, 0, 0, 0, 0, 0, 1}; */
/* double H[81] = {1, 0, 0, 0, 0, 0, 0, 0, 0, */
/* 0, 1, 0, 0, 0, 0, 0, 0, 0, */
/* 0, 0, 1, 0, 0, 0, 0, 0, 0, */
/* 0, 0, 0, 1, 0, 0, 0, 0, 0, */
/* 0, 0, 0, 0, 1, 0, 0, 0, 0, */
/* 0, 0, 0, 0, 0, 1, 0, 0, 0, */
/* 0, 0, 0, 0, 0, 0, 1, 0, 0, */
/* 0, 0, 0, 0, 0, 0, 0, 1, 0, */
/* 0, 0, 0, 0, 0, 0, 0, 0, 1}; */
/* double q[9] = {-1, 1, 3, -1, 1, 3, -1, 1, 3}; */
/* double b[9] = {0, 0, 0,0, 0, 0,0, 0, 0 }; */
/* double mu[3] = {0.1,0.1,0.1}; */
int k;
int m = 3 * NC;
int n = Ndof;
GlobalFrictionContactProblem numericsProblem;
globalFrictionContact_null(&numericsProblem);
numericsProblem.numberOfContacts = NC;
numericsProblem.dimension = 3;
numericsProblem.mu = mu;
numericsProblem.q = q;
numericsProblem.b = b;
numericsProblem.M = newNumericsMatrix();
NumericsMatrix *MM = numericsProblem.M;
MM->storageType = NM_SPARSE_BLOCK;
MM->size0 = Ndof;
MM->size1 = Ndof;
MM->matrix1 = newSBM();
SparseBlockStructuredMatrix *MBlockMatrix = MM->matrix1;
MBlockMatrix->nbblocks = 3;
double * block[3] = {M11, M22, M33};
MBlockMatrix->block = block;
MBlockMatrix->blocknumber0 = 3;
MBlockMatrix->blocknumber1 = 3;
unsigned int blocksize[3] = {3, 6, 9} ;
MBlockMatrix->blocksize0 = blocksize;
MBlockMatrix->blocksize1 = blocksize;
MBlockMatrix->filled1 = 4;
MBlockMatrix->filled2 = 3;
size_t index1_data[4] = {0, 1, 2, 3} ;
size_t index2_data[3] = {0, 1, 2} ;
MBlockMatrix->index1_data = index1_data;
MBlockMatrix->index2_data = index2_data;
numericsProblem.H = newNumericsMatrix();
NumericsMatrix *HH = numericsProblem.H;
HH->storageType = 1;
HH->size0 = Ndof;
HH->size1 = 3 * NC;
HH->matrix1 = (SparseBlockStructuredMatrix*)malloc(sizeof(SparseBlockStructuredMatrix));
SparseBlockStructuredMatrix *HBlockMatrix = HH->matrix1;
HBlockMatrix->nbblocks = 2;
double * hblock[3] = {H00, H20};
HBlockMatrix->block = hblock;
HBlockMatrix->blocknumber0 = 3;
HBlockMatrix->blocknumber1 = 1;
unsigned int blocksize0[3] = {3, 6, 9} ;
unsigned int blocksize1[1] = {3} ;
HBlockMatrix->blocksize0 = blocksize0;
HBlockMatrix->blocksize1 = blocksize1;
HBlockMatrix->filled1 = 4;
HBlockMatrix->filled2 = 2;
size_t hindex1_data[4] = {0, 1, 1, 2} ;
size_t hindex2_data[3] = {0, 0} ;
HBlockMatrix->index1_data = hindex1_data;
HBlockMatrix->index2_data = hindex2_data;
FILE * foutput = fopen("Example_GlobalFrictionContact_SBM.dat", "w");
globalFrictionContact_printInFile(&numericsProblem, foutput);
fclose(foutput);
// Unknown Declaration
double *reaction = (double*)calloc(m, sizeof(double));
double *velocity = (double*)calloc(m, sizeof(double));
double *globalVelocity = (double*)calloc(n, sizeof(double));
// Solver Options
SolverOptions * numerics_solver_options = (SolverOptions *)malloc(sizeof(SolverOptions));
// char solvername[10]= "NSGS";
/*\warning Must be adpated for future globalFrictionContact3D_setDefaultSolverOptions*/
gfc3d_setDefaultSolverOptions(numerics_solver_options, SICONOS_GLOBAL_FRICTION_3D_NSGS);
numerics_solver_options->dparam[0] = 1e-14;
numerics_solver_options->iparam[0] = 100000;
//Driver call
info = gfc3d_driver(&numericsProblem,
reaction , velocity, globalVelocity,
numerics_solver_options);
solver_options_delete(numerics_solver_options);
free(numerics_solver_options);
// Solver output
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");
free(reaction);
free(velocity);
free(globalVelocity);
// freeSBM(MM->matrix1);
// freeSBM(HH->matrix1);
free(MM->matrix1);
MM->matrix1 = NULL;
free(HH->matrix1);
HH->matrix1 = NULL;
freeNumericsMatrix(MM);
freeNumericsMatrix(HH);
free(MM);
free(HH);
gfc3d_free_workspace(&numericsProblem);
/* while (1) sleep(60); */
return info;
}
static int globalFrictionContact3D_AVI_gams_base(GlobalFrictionContactProblem* problem, double *reaction, double *velocity, SolverOptions* options, const char* solverName)
{
assert(problem);
assert(problem->numberOfContacts > 0);
assert(problem->M);
assert(problem->q);
/* Handles to the GAMSX, GDX, and Option objects */
gamsxHandle_t Gptr = NULL;
idxHandle_t Xptr = NULL;
optHandle_t Optr = NULL;
optHandle_t solverOptPtr = NULL;
int status;
char sysdir[GMS_SSSIZE], model[GMS_SSSIZE], msg[GMS_SSSIZE];
const char defModel[] = SPACE_CONC(GAMS_MODELS_SHARE_DIR, "/fc_vi.gms");
const char defGAMSdir[] = GAMS_DIR;
int size = problem->dimension*problem->numberOfContacts;
NumericsMatrix Htmat;
fillNumericsMatrix(&Htmat, NM_SPARSE, problem->H->size0, problem->H->size1, NULL);
SN_Gams_set_dirs(options->solverParameters, defModel, defGAMSdir, model, sysdir, "/fc_vi.gms");
/* Create objects */
if (! gamsxCreateD (&Gptr, sysdir, msg, sizeof(msg))) {
printf("Could not create gamsx object: %s\n", msg);
return 1;
}
if (! idxCreateD (&Xptr, sysdir, msg, sizeof(msg))) {
printf("Could not create gdx object: %s\n", msg);
return 1;
}
if (! optCreateD (&Optr, sysdir, msg, sizeof(msg))) {
printf("Could not create opt object: %s\n", msg);
return 1;
}
if (! optCreateD (&solverOptPtr, sysdir, msg, sizeof(msg))) {
printf("Could not create opt object: %s\n", msg);
return 1;
}
getGamsSolverOpt(solverOptPtr, sysdir, solverName);
optSetDblStr(solverOptPtr, "convergence_tolerance", options->dparam[0]);
// strncpy(msg, "./", sizeof(deffile));
strncpy(msg, solverName, sizeof(msg));
strncat(msg, ".opt", sizeof(msg));
optWriteParameterFile(solverOptPtr, msg);
FILE* f = fopen("jams.opt", "w");
if (f)
{
char contents[] = "subsolveropt 1";
fprintf(f, contents);
fclose(f);
}
else
{
printf("Failed to create jams.opt!\n");
}
getGamsOpt(Optr, sysdir);
if (strcmp(solverName, "path"))
{
optSetStrStr(Optr, "emp", solverName);
}
idxOpenWrite(Xptr, "fc3d_avi.gdx", "Siconos/Numerics NM_to_GDX", &status);
if (status)
idxerrorR(status, "idxOpenWrite");
DEBUG_PRINT("GFC3D_AVI_GAMS :: fc3d_avi.gdx opened");
if ((status=NM_to_GDX(Xptr, "M", "M matrix", problem->M))) {
printf("Model data not written\n");
goto TERMINATE;
}
DEBUG_PRINT("FC3D_AVI_GAMS :: M matrix written");
if ((status=NM_to_GDX(Xptr, "H", "H matrix", problem->H))) {
printf("Model data not written\n");
goto TERMINATE;
}
DEBUG_PRINT("FC3D_AVI_GAMS :: H matrix written");
NM_copy_to_sparse(problem->H, &Htmat);
cs_fkeep(NM_csc(&Htmat), &SN_rm_normal_part, NULL);
cblas_dcopy(size, problem->b, 1, reaction, 1);
for (unsigned i = 0; i < size; i += 3)
{
reaction[i] = 0.;
}
if ((status=NM_to_GDX(Xptr, "Ht", "Ht matrix", &Htmat))) {
printf("Model data not written\n");
goto TERMINATE;
}
if ((status=NV_to_GDX(Xptr, "q", "q vector", problem->q, size))) {
printf("Model data not written\n");
goto TERMINATE;
}
if ((status=NV_to_GDX(Xptr, "b", "b vector", problem->b, size))) {
printf("Model data not written\n");
goto TERMINATE;
}
if ((status=NV_to_GDX(Xptr, "bt", "bt vector", reaction, size))) {
printf("Model data not written\n");
goto TERMINATE;
}
if (idxClose(Xptr))
idxerrorR(idxGetLastError(Xptr), "idxClose");
if ((status=CallGams(Gptr, Optr, sysdir, model))) {
printf("Call to GAMS failed\n");
goto TERMINATE;
}
/************************************************
* Read back solution
************************************************/
idxOpenRead(Xptr, "fc3d_avi_sol.gdx", &status);
if (status)
idxerrorR(status, "idxOpenRead");
if ((status=GDX_to_NV(Xptr, "reaction", reaction, size))) {
printf("Model data not read\n");
goto TERMINATE;
}
if ((status=GDX_to_NV(Xptr, "velocities", reaction, size))) {
printf("Model data not read\n");
goto TERMINATE;
}
if (idxClose(Xptr))
idxerrorR(idxGetLastError(Xptr), "idxClose");
TERMINATE:
optFree(&Optr);
optFree(&solverOptPtr);
idxFree(&Xptr);
gamsxFree(&Gptr);
freeNumericsMatrix(&Htmat);
return status;
}
int avi_caoferris(AffineVariationalInequalities* problem, double *z, double *w, SolverOptions* options)
{
unsigned n = problem->size;
assert(n > 0);
unsigned nrows = problem->poly->size_ineq;
assert(nrows - n > 0);
unsigned n_I = nrows - n; /* Number of inactive constraints */
/* Create the data problem */
LinearComplementarityProblem lcplike_pb;
lcplike_pb.size = nrows;
NumericsMatrix num_mat;
fillNumericsMatrix(&num_mat, NM_DENSE, nrows, nrows, calloc(nrows*nrows, sizeof(double)));
lcplike_pb.M = &num_mat;
lcplike_pb.q = (double *)calloc(nrows, sizeof(double));
double* a_bar = (double *)malloc(nrows*sizeof(double));
double* B_A_T = (double*)malloc(n*n*sizeof(double));
double* copyA = (double*)malloc(n*n*sizeof(double));
double* B_I_T = (double*)malloc(n*(n_I)*sizeof(double));
double* d_vec = (double *)malloc(nrows*sizeof(double));
int* basis = (int *)malloc((2*nrows+1)*sizeof(int));
siconos_find_vertex(problem->poly, n, basis);
DEBUG_PRINT_VEC_INT(basis, nrows+1);
const double* H = problem->poly->H;
const double* K = problem->poly->K;
/* Set of active constraints */
unsigned* A = (unsigned*)malloc(n*sizeof(unsigned));
int* active_constraints = &basis[nrows+1];
/* set active_constraints to 1 at the beginning */
memset(active_constraints, -1, nrows*sizeof(int));
DEBUG_PRINT_VEC_INT(active_constraints, nrows);
unsigned indx_B_I_T = 0;
for (unsigned i = 1; i <= nrows; ++i)
{
assert((unsigned)abs(basis[i]) > nrows); /* we don't want slack variable here */
int indx = abs(basis[i]) - nrows - 1 - n;
if (indx >= 0)
{
/* this is an inactive constraint */
assert(indx_B_I_T < n_I);
assert((unsigned)indx < nrows);
cblas_dcopy(n, &H[indx], nrows, &B_I_T[indx_B_I_T*n], 1); /* form B_I_T */
active_constraints[indx] = 0; /* desactivate the constraint */
lcplike_pb.q[n+indx_B_I_T] = -K[indx]; /* partial construction of q[n:nrows] as -K_I */
indx_B_I_T++;
}
}
DEBUG_PRINT_VEC_INT(active_constraints, nrows);
unsigned indx_B_A_T = 0;
for (unsigned i = 0; i < nrows; ++i)
{
if (active_constraints[i] == -1)
{
assert(indx_B_A_T < n);
A[indx_B_A_T] = i+1; /* note which constraints is active */
cblas_dcopy(n, &H[i], nrows, &B_A_T[indx_B_A_T*n], 1); /* form B_A_T */
d_vec[indx_B_A_T] = K[i]; /* save K_A */
indx_B_A_T++;
}
}
assert(indx_B_A_T == n && "there were not enough active constraints");
DEBUG_PRINT_VEC_STR("K_A", d_vec, n);
cblas_dcopy(n*n, problem->M->matrix0, 1, copyA, 1);
DEBUG_PRINT_MAT(B_A_T, n, n);
DEBUG_PRINT_MAT(B_I_T, n, n_I);
/* get LU for B_A_T */
int* ipiv = basis;
int infoLAPACK = 0;
/* LU factorisation of B_A_T */
DGETRF(n, n, B_A_T, n, ipiv, &infoLAPACK);
assert(infoLAPACK <= 0 && "avi_caoferris :: info from DGETRF > 0, this should not append !\n");
/* compute B_A_T^{-1}B_I_T */
DGETRS(LA_NOTRANS, n, n_I, B_A_T, n, ipiv, B_I_T, n, &infoLAPACK);
assert(infoLAPACK == 0 && "avi_caoferris :: info from DGETRS for solving B_A_T X = B_I_T is not zero!\n");
DEBUG_PRINT("B_A_T^{-1}B_I_T\n");
DEBUG_PRINT_MAT(B_I_T, n, n_I);
/* Compute B_A_T^{-1} A */
DGETRS(LA_NOTRANS, n, n, B_A_T, n, ipiv, copyA, n, &infoLAPACK);
assert(infoLAPACK == 0 && "avi_caoferris :: info from DGETRS for solving B_A_T X = A is not zero!\n");
DEBUG_PRINT("B_A_T^{-1}A\n");
DEBUG_PRINT_MAT(copyA, n, n);
/* do some precomputation for \bar{q}: B_A_T^{-1}q_{AVI} */
cblas_dcopy_msan(n, problem->q, 1, a_bar, 1);
DGETRS(LA_NOTRANS, n, 1, B_A_T, n, ipiv, a_bar, n, &infoLAPACK);
assert(infoLAPACK == 0 && "avi_caoferris :: info from DGETRS for solving B_A_T X = a_bar is not zero!\n");
DEBUG_PRINT_VEC_STR("B_A_T{-1}q_{AVI}", a_bar, n);
/* Do the transpose of B_A_T^{-1} A */
double* basepointer = &num_mat.matrix0[nrows*nrows - n*n];
for (unsigned i = 0; i < n; ++i) cblas_dcopy(n, ©A[i*n], 1, &basepointer[i], n);
/* Compute B_A_T^{-1}(B_A_T^{-1}M)_T */
DGETRS(LA_NOTRANS, n, n, B_A_T, n, ipiv, basepointer, n, &infoLAPACK);
assert(infoLAPACK == 0 && "avi_caoferris :: info from DGETRS for solving B_A_T X = (B_A_T^{-1}M)_T is not zero!\n");
DEBUG_PRINT("B_A_T^{-1}(B_A_T^{-1}M)_T\n");
DEBUG_PRINT_MAT(basepointer, n, n);
for (unsigned i = 0; i < n; ++i) cblas_dcopy(n, &basepointer[n*i], 1, ©A[i], n);
DEBUG_PRINT_VEC_STR("b_I =: q[n:nrows]", (&lcplike_pb.q[n]), n_I);
/* partial construction of q: q[n:nrows] += (B_A_T^{-1}*B_I_T)_T K_A */
cblas_dgemv(CblasColMajor, CblasTrans, n_I, n, 1.0, B_I_T, n_I, d_vec, 1, 1.0, &lcplike_pb.q[n], 1);
DEBUG_PRINT_VEC_STR("final q[n:nrows] as b_I + B_I B_A^{-1}b_A", (&lcplike_pb.q[n]), n_I);
/* Compute B_A_T^{-1} M B_A^{-1} K_A
* We have to set CblasTrans since we still have a transpose */
/* XXX It looks like we could have 2 here, but not it does not work w/ it. Investigate why -- xhub */
cblas_dgemv(CblasColMajor, CblasTrans, n, n, 1.0, basepointer, n, d_vec, 1, 0.0, lcplike_pb.q, 1);
DEBUG_PRINT_VEC_STR("B_A_T^{-1} M B_A^{-1} K_A =: q[0:n]", lcplike_pb.q, n);
/* q[0:n] = 2 B_A_T^{-1} A B_A^{-1}b_A + B_A_T{-1} q_{AVI} */
/* XXX about the + or -: we do not follow the convention of Cao & Ferris */
cblas_daxpy(n, 1.0, a_bar, 1, lcplike_pb.q, 1);
DEBUG_PRINT("final q\n");
DEBUG_PRINT_VEC(lcplike_pb.q, nrows);
/* q is now ready, let's deal with M */
/* set some pointers to sub-matrices */
double* upper_left_mat = num_mat.matrix0;
double* upper_right_mat = &num_mat.matrix0[n*nrows];
double* lower_left_mat = &num_mat.matrix0[n];
double* lower_right_mat = &upper_right_mat[n];
/* copy the B_A_T^{-1} B_I_T (twice) and set the lower-left part to 0*/
for (unsigned i = 0, j = 0, k = 0; i < n_I; ++i, j += n_I, k += nrows)
{
cblas_dcopy(n, ©A[n*i], 1, &upper_right_mat[k], 1);/* copy into the right location B_A_T^{-1} M B_A^{-1} */
cblas_dcopy(n_I, &B_I_T[j], 1, &upper_left_mat[k], 1); /* copy B_A_T^{-1}*B_I_T to the upper-right block */
cblas_dscal(n, -1.0, &upper_left_mat[k], 1); /* take the opposite of the matrix */
cblas_dcopy(n_I, &B_I_T[j], 1, &lower_right_mat[i], nrows); /* copy B_IB_A^{-1} to the lower-left block */
memset(&lower_left_mat[k], 0, sizeof(double)*(n_I)); /* set the lower-left block to 0 */
}
DEBUG_PRINT_MAT(num_mat.matrix0, nrows, nrows);
/* Matrix M is now ready */
/* Save K_A */
double* K_A = a_bar;
cblas_dcopy(n, d_vec, 1, K_A, 1);
DEBUG_PRINT_VEC(K_A, n);
/* We put -1 because we directly copy it in stage 3 */
for (unsigned int i = 0; i < n; ++i) d_vec[i] = -1.0;
memset(&d_vec[n], 0, n_I*sizeof(double));
DEBUG_PRINT_VEC_INT_STR("Active set", A, n);
double* u_vec = (double *)calloc(nrows, sizeof(double));
double* s_vec = (double *)calloc(nrows, sizeof(double));
/* Call directly the 3rd stage
* Here w is used as u and z as s in the AVI */
int info = avi_caoferris_stage3(&lcplike_pb, u_vec, s_vec, d_vec, n, A, options);
/* Update z */
/* XXX why no w ? */
DEBUG_PRINT_VEC_INT(A, n);
for (unsigned i = 0; i < n; ++i) z[i] = s_vec[A[i]-1] + K_A[i];
DEBUG_PRINT_VEC_STR("s_A + K_A", z, n);
DGETRS(LA_TRANS, n, 1, B_A_T, n, ipiv, z, n, &infoLAPACK);
assert(infoLAPACK == 0 && "avi_caoferris :: info from DGETRS for solving B_A X = s_A + K_A is not zero!\n");
DEBUG_PRINT_VEC_STR("solution z", z, n);
/* free allocated stuff */
free(u_vec);
free(s_vec);
free(A);
free(basis);
free(d_vec);
free(B_I_T);
free(copyA);
free(B_A_T);
freeNumericsMatrix(lcplike_pb.M);
free(lcplike_pb.q);
free(a_bar);
return info;
}
void fc3d_nonsmooth_Newton_solvers_solve(fc3d_nonsmooth_Newton_solvers* equation,
double* reaction,
double* velocity,
int* info,
SolverOptions* options)
{
assert(equation);
FrictionContactProblem* problem = equation->problem;
assert(problem);
assert(reaction);
assert(velocity);
assert(info);
assert(options);
assert(problem->dimension == 3);
assert(options->iparam);
assert(options->dparam);
assert(problem->q);
assert(problem->mu);
assert(problem->M);
assert(problem->M->matrix0 || problem->M->matrix1 || problem->M->matrix2);
assert(!options->iparam[4]); // only host
unsigned int problemSize = 3 * problem->numberOfContacts;
unsigned int iter = 0;
unsigned int itermax = options->iparam[0];
unsigned int erritermax = options->iparam[7];
int nzmax;
if (problem->M->storageType == NM_DENSE)
{
nzmax = problemSize * problemSize;
}
else
{
nzmax = options->iparam[3];
}
assert(itermax > 0);
assert(nzmax > 0);
double tolerance = options->dparam[0];
assert(tolerance > 0);
if (verbose > 0)
printf("------------------------ FC3D - _nonsmooth_Newton_solversSolve - Start with tolerance = %g\n", tolerance);
unsigned int _3problemSize = 3 * problemSize;
double normq = cblas_dnrm2(problemSize , problem->q , 1);
void *buffer;
if (!options->dWork)
{
buffer = malloc((11 * problemSize) * sizeof(double)); // F(1),
// tmp1(1),
// tmp2(1),
// tmp3(1),
// A(3),
// B(3), rho
}
else
{
buffer = options->dWork;
}
double *F = (double *) buffer;
double *tmp1 = (double *) F + problemSize;
double *tmp2 = (double *) tmp1 + problemSize;
double *tmp3 = (double *) tmp2 + problemSize;
double *Ax = tmp3 + problemSize;
double *Bx = Ax + _3problemSize;
double *rho = Bx + _3problemSize;
NumericsMatrix *AWpB, *AWpB_backup;
if (!options->dWork)
{
AWpB = createNumericsMatrix(problem->M->storageType,
problem->M->size0, problem->M->size1);
AWpB_backup = createNumericsMatrix(problem->M->storageType,
problem->M->size0, problem->M->size1);
}
else
{
AWpB = (NumericsMatrix*) (rho + problemSize);
AWpB_backup = (NumericsMatrix*) (AWpB + sizeof(NumericsMatrix*));
}
/* just for allocations */
NM_copy(problem->M, AWpB);
if (problem->M->storageType != NM_DENSE)
{
switch(options->iparam[13])
{
case 0:
{
NM_linearSolverParams(AWpB)->solver = NS_CS_LUSOL;
break;
}
case 1:
{
NM_linearSolverParams(AWpB)->solver = NS_MUMPS;
#ifdef HAVE_MPI
assert (options->solverData);
if ((MPI_Comm) options->solverData == MPI_COMM_NULL)
{
options->solverData = NM_MPI_com(MPI_COMM_NULL);
}
else
{
NM_MPI_com((MPI_Comm) options->solverData);
}
#endif
break;
}
default:
{
numerics_error("fc3d_nonsmooth_Newton_solvers_solve", "Unknown linear solver.\n");
}
}
}
// compute rho here
for (unsigned int i = 0; i < problemSize; ++i) rho[i] = options->dparam[3];
// velocity <- M*reaction + qfree
cblas_dcopy(problemSize, problem->q, 1, velocity, 1);
NM_gemv(1., problem->M, reaction, 1., velocity);
double linear_solver_residual=0.0;
while (iter++ < itermax)
{
equation->function(equation->data,
problemSize,
reaction, velocity, equation->problem->mu,
rho,
F, Ax, Bx);
// AW + B
computeAWpB(Ax, problem->M, Bx, AWpB);
cblas_dcopy_msan(problemSize, F, 1, tmp1, 1);
cblas_dscal(problemSize, -1., tmp1, 1);
/* Solve: AWpB X = -F */
NM_copy(AWpB, AWpB_backup);
int lsi = NM_gesv(AWpB, tmp1);
/* NM_copy needed here */
NM_copy(AWpB_backup, AWpB);
if (lsi)
{
if (verbose > 0)
{
numerics_warning("fc3d_nonsmooth_Newton_solvers_solve",
"warning! linear solver exit with code = %d\n", lsi);
}
}
if (verbose > 0)
{
cblas_dcopy_msan(problemSize, F, 1, tmp3, 1);
NM_gemv(1., AWpB, tmp1, 1., tmp3);
linear_solver_residual = cblas_dnrm2(problemSize, tmp3, 1);
/* fprintf(stderr, "fc3d esolve: linear equation residual = %g\n", */
/* cblas_dnrm2(problemSize, tmp3, 1)); */
/* for the component wise scaled residual: cf mumps &
* http://www.netlib.org/lapack/lug/node81.html */
}
// line search
double alpha = 1;
int info_ls = 0;
cblas_dcopy_msan(problemSize, tmp1, 1, tmp3, 1);
switch (options->iparam[11])
{
case -1:
/* without line search */
info_ls = 1;
break;
case 0:
/* Goldstein Price */
info_ls = globalLineSearchGP(equation, reaction, velocity, problem->mu, rho, F, Ax, Bx, problem->M, problem->q, AWpB, tmp1, tmp2, &alpha, options->iparam[12]);
break;
case 1:
/* FBLSA */
info_ls = frictionContactFBLSA(equation, reaction, velocity, problem->mu, rho, F, Ax, Bx,
problem->M, problem->q, AWpB, tmp1, tmp2, &alpha, options->iparam[12]);
break;
default:
{
numerics_error("fc3d_nonsmooth_Newton_solvers_solve",
"Unknown line search option.\n");
}
}
if (!info_ls)
// tmp2 should contains the reaction iterate of the line search
// for GP this should be the same as cblas_daxpy(problemSize, alpha, tmp1, 1, reaction, 1);
cblas_dcopy(problemSize, tmp2, 1, reaction, 1);
else
cblas_daxpy(problemSize, 1., tmp3, 1., reaction, 1);
// velocity <- M*reaction + qfree
cblas_dcopy(problemSize, problem->q, 1, velocity, 1);
NM_gemv(1., problem->M, reaction, 1., velocity);
options->dparam[1] = INFINITY;
if (!(iter % erritermax))
{
fc3d_compute_error(problem, reaction, velocity,
// fc3d_FischerBurmeister_compute_error(problem, reaction, velocity,
tolerance, options, normq, &(options->dparam[1]));
DEBUG_EXPR_WE(equation->function(equation->data, problemSize,
reaction, velocity, equation->problem->mu, rho,
F, NULL, NULL));
DEBUG_EXPR_WE(assert((cblas_dnrm2(problemSize, F, 1)
/ (1 + cblas_dnrm2(problemSize, problem->q, 1)))
<= (10 * options->dparam[1] + 1e-15)));
}
if (verbose > 0)
{
equation->function(equation->data, problemSize,
reaction, velocity, equation->problem->mu, rho,
F, NULL, NULL);
printf(" ---- fc3d_nonsmooth_Newton_solvers_solve: iteration %d : , linear solver residual =%g, residual=%g, ||F||=%g\n", iter, linear_solver_residual, options->dparam[1],cblas_dnrm2(problemSize, F, 1));
}
if (options->callback)
{
options->callback->collectStatsIteration(options->callback->env, problemSize, reaction, velocity,
options->dparam[1], NULL);
}
if (isnan(options->dparam[1]))
{
if (verbose > 0)
{
printf(" fc3d_nonsmooth_Newton_solvers_solve: iteration %d : computed residual is not a number, stop.\n", iter);
}
info[0] = 2;
break;
}
if (options->dparam[1] < tolerance)
{
info[0] = 0;
break;
}
}
if (verbose > 0)
{
if (!info[0])
printf("------------------------ FC3D - NSN - convergence after %d iterations, residual : %g < %g \n", iter, options->dparam[1],tolerance);
else
{
printf("------------------------ FC3D - NSN - no convergence after %d iterations, residual : %g < %g \n", iter, options->dparam[1], tolerance);
}
}
options->iparam[SICONOS_IPARAM_ITER_DONE] = iter;
if (!options->dWork)
{
assert(buffer);
free(buffer);
options->dWork = NULL;
}
else
{
assert(buffer == options->dWork);
}
if (!options->dWork)
{
freeNumericsMatrix(AWpB);
freeNumericsMatrix(AWpB_backup);
free(AWpB);
free(AWpB_backup);
}
if (verbose > 0)
printf("------------------------ FC3D - NSN - End\n");
}
void ncp_pathsearch(NonlinearComplementarityProblem* problem, double* z, double* F, int *info , SolverOptions* options)
{
/* Main step of the algorithm:
* - compute jacobians
* - call modified lemke
*/
unsigned int n = problem->n;
unsigned int preAlloc = options->iparam[SICONOS_IPARAM_PREALLOC];
int itermax = options->iparam[SICONOS_IPARAM_MAX_ITER];
double merit_norm = 1.0;
double nn_tol = options->dparam[SICONOS_DPARAM_TOL];
int nbiter = 0;
/* declare a LinearComplementarityProblem on the stack*/
LinearComplementarityProblem lcp_subproblem;
lcp_subproblem.size = n;
/* do some allocation if required
* - nabla_F (used also as M for the LCP subproblem)
* - q for the LCP subproblem
*
* Then fill the LCP subproblem
*/
if (!preAlloc || (preAlloc && !options->internalSolvers))
{
options->internalSolvers = (SolverOptions *) malloc(sizeof(SolverOptions));
solver_options_set(options->internalSolvers, SICONOS_LCP_PIVOT);
options->numberOfInternalSolvers = 1;
SolverOptions * lcp_options = options->internalSolvers;
/* We always allocation once and for all since we are supposed to solve
* many LCPs */
lcp_options->iparam[SICONOS_IPARAM_PREALLOC] = 1;
/* set the right pivot rule */
lcp_options->iparam[SICONOS_IPARAM_PIVOT_RULE] = SICONOS_LCP_PIVOT_PATHSEARCH;
/* set the right stacksize */
lcp_options->iparam[SICONOS_IPARAM_PATHSEARCH_STACKSIZE] = options->iparam[SICONOS_IPARAM_PATHSEARCH_STACKSIZE];
}
assert(problem->nabla_F);
lcp_subproblem.M = problem->nabla_F;
if (!preAlloc || (preAlloc && !options->dWork))
{
options->dWork = (double *) malloc(4*n*sizeof(double));
}
lcp_subproblem.q = options->dWork;
double* x = &options->dWork[n];
double* x_plus = &options->dWork[2*n];
double* r = &options->dWork[3*n];
NMS_data* data_NMS;
functions_LSA* functions;
if (!preAlloc || (preAlloc && !options->solverData))
{
options->solverData = malloc(sizeof(pathsearch_data));
pathsearch_data* solverData = (pathsearch_data*) options->solverData;
/* do all the allocation */
solverData->data_NMS = create_NMS_data(n, NM_DENSE, options->iparam, options->dparam);
solverData->lsa_functions = (functions_LSA*) malloc(sizeof(functions_LSA));
solverData->data_NMS->set = malloc(sizeof(positive_orthant));
data_NMS = solverData->data_NMS;
functions = solverData->lsa_functions;
/* for use in NMS; only those 3 functions are called */
init_lsa_functions(functions, &FB_compute_F_ncp, &ncp_FB);
functions->compute_H = &FB_compute_H_ncp;
set_set_id(data_NMS->set, SICONOS_SET_POSITIVE_ORTHANT);
/* fill ls_data */
data_NMS->ls_data->compute_F = functions->compute_F;
data_NMS->ls_data->compute_F_merit = functions->compute_F_merit;
data_NMS->ls_data->z = NULL; /* XXX to check -- xhub */
data_NMS->ls_data->zc = NMS_get_generic_workV(data_NMS->workspace, n);
data_NMS->ls_data->F = NMS_get_F(data_NMS->workspace, n);
data_NMS->ls_data->F_merit = NMS_get_F_merit(data_NMS->workspace, n);
data_NMS->ls_data->desc_dir = NMS_get_dir(data_NMS->workspace, n);
/** \todo this value should be settable by the user with a default value*/
data_NMS->ls_data->alpha_min = fmin(data_NMS->alpha_min_watchdog, data_NMS->alpha_min_pgrad);
data_NMS->ls_data->data = (void*)problem;
data_NMS->ls_data->set = data_NMS->set;
data_NMS->ls_data->sigma = options->dparam[SICONOS_DPARAM_NMS_SIGMA];
/* data_NMS->ls_data->searchtype is set in the NMS code */
}
else
{
pathsearch_data* solverData = (pathsearch_data*) options->solverData;
data_NMS = solverData->data_NMS;
functions = solverData->lsa_functions;
}
/* initial value for ref_merit */
problem->compute_F(problem->env, n, z, F);
functions->compute_F_merit(problem, z, F, data_NMS->ls_data->F_merit);
data_NMS->ref_merit = .5 * cblas_ddot(n, data_NMS->ls_data->F_merit, 1, data_NMS->ls_data->F_merit, 1);
data_NMS->merit_bestpoint = data_NMS->ref_merit;
cblas_dcopy(n, z, 1, NMS_checkpoint_0(data_NMS, n), 1);
cblas_dcopy(n, z, 1, NMS_checkpoint_T(data_NMS, n), 1);
cblas_dcopy(n, z, 1, NMS_bestpoint(data_NMS, n), 1);
/* -------------------- end init ---------------------------*/
int nms_failed = 0;
double err = 10*nn_tol;
/* to check the solution */
LinearComplementarityProblem lcp_subproblem_check;
int check_lcp_solution = 1; /* XXX add config for that */
double normal_norm2_newton_point;
/* F is already computed here at z */
while ((err > nn_tol) && (nbiter < itermax) && !nms_failed)
{
int force_watchdog_step = 0;
int force_d_step_merit_check = 0;
double check_ratio = 0.0;
nbiter++;
/* update M, q and r */
/* First find x */
ncp_pathsearch_compute_x_from_z(n, z, F, x);
pos_part(n, x, x_plus); /* update x_plus */
ncp_pathsearch_update_lcp_data(problem, &lcp_subproblem, n, x_plus, x, r);
if (check_lcp_solution)
{
lcp_subproblem_check.size = n;
lcp_subproblem_check.M = problem->nabla_F;
lcp_subproblem_check.q = lcp_subproblem.q;
//cblas_dcopy(n, x, 1, lcp_subproblem_check.q , 1);
//prodNumericsMatrix(n, n, -1.0, problem->nabla_F, x_plus, 0.0, lcp_subproblem.q);
}
double norm_r2 = cblas_ddot(n, r, 1, r, 1);
if (norm_r2 < DBL_EPSILON*DBL_EPSILON) /* ||r|| < 1e-15 */
{
DEBUG_PRINTF("ncp_pathsearch :: ||r|| = %e < %e; path search procedure was successful!\n", norm_r2, DBL_EPSILON*DBL_EPSILON);
(*info) = 0;
ncp_compute_error(n, z, F, nn_tol, &err); /* XXX F should be up-to-date, we should check only CC*/
break;
}
/* end update M, q and r */
lcp_pivot_covering_vector(&lcp_subproblem, x_plus, x, info, options->internalSolvers, r);
switch (*info)
{
case LCP_PIVOT_SUCCESS:
DEBUG_PRINT("ncp_pathsearch :: path search procedure was successful!\n");
if (check_lcp_solution)
{
double err_lcp = 0.0;
cblas_daxpy(n, 1.0, r, 1, lcp_subproblem_check.q, 1);
lcp_compute_error(&lcp_subproblem_check, x_plus, x, 1e-14, &err_lcp);
double local_tol = fmax(1e-14, DBL_EPSILON*sqrt(norm_r2));
printf("ncp_pathsearch :: lcp solved with error = %e; local_tol = %e\n", err_lcp, local_tol);
//assert(err_lcp < local_tol && "ncp_pathsearch :: lcp solved with very bad precision");
if (err_lcp > local_tol)
{
printf("ncp_pathsearch :: lcp solved with very bad precision\n");
NM_display(lcp_subproblem.M);
printf("z r q x_plus\n");
for (unsigned i = 0; i < n; ++i) printf("%e %e %e %e\n", z[i], r[i], lcp_subproblem.q[i], x_plus[i]);
options->internalSolvers->iparam[SICONOS_IPARAM_PIVOT_RULE] = 0;
lcp_pivot(&lcp_subproblem, x_plus, x, info, options->internalSolvers);
options->internalSolvers->iparam[SICONOS_IPARAM_PIVOT_RULE] = SICONOS_LCP_PIVOT_PATHSEARCH;
lcp_compute_error(&lcp_subproblem_check, x_plus, x, 1e-14, &err_lcp);
printf("ncp_pathsearch :: lcp resolved with error = %e; local_tol = %e\n", err_lcp, local_tol);
}
/* XXX missing recompute x ?*/
/* recompute the normal norm */
problem->compute_F(problem->env, n, x_plus, r);
cblas_daxpy(n, -1.0, x, 1, r, 1);
normal_norm2_newton_point = cblas_ddot(n, r, 1, r, 1);
if (normal_norm2_newton_point > norm_r2)
{
printf("ncp_pathsearch :: lcp successfully solved, but the norm of the normal map increased! %e > %e\n", normal_norm2_newton_point, norm_r2);
//assert(normal_norm2_newton_point <= norm_r2);
}
else
{
printf("ncp_pathsearch :: lcp successfully solved, norm of the normal map decreased! %e < %e\n", normal_norm2_newton_point, norm_r2);
//check_ratio = norm_r2/normal_norm2_newton_point;
}
if (50*normal_norm2_newton_point < norm_r2)
{
force_d_step_merit_check = 1;
}
else if (10*normal_norm2_newton_point < norm_r2)
{
// check_ratio = sqrt(norm_r2/normal_norm2_newton_point);
}
}
break;
case LCP_PIVOT_RAY_TERMINATION:
DEBUG_PRINT("ncp_pathsearch :: ray termination, let's fastened your seat belt!\n");
break;
case LCP_PATHSEARCH_LEAVING_T:
DEBUG_PRINT("ncp_pathsearch :: leaving t, fastened your seat belt!\n");
DEBUG_PRINTF("ncp_pathsearch :: max t value = %e\n", options->internalSolvers->dparam[2]); /* XXX fix 2 */
/* try to retry solving the problem */
/* XXX keep or not ? */
/* recompute the normal norm */
problem->compute_F(problem->env, n, x_plus, r);
cblas_daxpy(n, -1.0, x, 1, r, 1);
normal_norm2_newton_point = cblas_ddot(n, r, 1, r, 1);
if (normal_norm2_newton_point > norm_r2)
{
printf("ncp_pathsearch :: lcp successfully solved, but the norm of the normal map increased! %e > %e\n", normal_norm2_newton_point, norm_r2);
//assert(normal_norm2_newton_point <= norm_r2);
}
else
{
printf("ncp_pathsearch :: lcp successfully solved, norm of the normal map decreased! %e < %e\n", normal_norm2_newton_point, norm_r2);
check_ratio = 5.0*norm_r2/normal_norm2_newton_point;
}
if (options->internalSolvers->dparam[2] > 1e-5) break;
memset(x_plus, 0, sizeof(double) * n);
problem->compute_F(problem->env, n, x_plus, r);
ncp_pathsearch_compute_x_from_z(n, x_plus, r, x);
ncp_pathsearch_update_lcp_data(problem, &lcp_subproblem, n, x_plus, x, r);
lcp_pivot_covering_vector(&lcp_subproblem, x_plus, x, info, options->internalSolvers, r);
if (*info == LCP_PIVOT_SUCCESS)
{
DEBUG_PRINT("ncp_pathsearch :: Lemke start worked !\n");
double err_lcp = 0.0;
cblas_daxpy(n, 1.0, r, 1, lcp_subproblem_check.q, 1);
lcp_compute_error(&lcp_subproblem_check, x_plus, x, 1e-14, &err_lcp);
double local_tol = fmax(1e-14, DBL_EPSILON*sqrt(norm_r2));
printf("ncp_pathsearch :: lcp solved with error = %e; local_tol = %e\n", err_lcp, local_tol);
assert(err_lcp < local_tol);
}
else
{
NM_display(lcp_subproblem.M);
printf("z r q x_plus\n");
for (unsigned i = 0; i < n; ++i) printf("%e %e %e %e\n", z[i], r[i], lcp_subproblem.q[i], x_plus[i]);
DEBUG_PRINT("ncp_pathsearch :: Lemke start did not succeeded !\n");
lcp_pivot_diagnose_info(*info);
if (*info == LCP_PATHSEARCH_LEAVING_T)
{
DEBUG_PRINTF("ncp_pathsearch :: max t value after Lemke start = %e\n", options->internalSolvers->dparam[2]);
}
options->internalSolvers->iparam[SICONOS_IPARAM_PIVOT_RULE] = 0;
lcp_pivot(&lcp_subproblem, x_plus, x, info, options->internalSolvers);
options->internalSolvers->iparam[SICONOS_IPARAM_PIVOT_RULE] = SICONOS_LCP_PIVOT_PATHSEARCH;
double err_lcp = 0.0;
lcp_compute_error(&lcp_subproblem, x_plus, x, 1e-14, &err_lcp);
printf("ncp_pathsearch :: lemke start resolved with info = %d; error = %e\n", *info, err_lcp);
printf("x_plus x_minus\n");
for (unsigned i = 0; i < n; ++i) printf("%e %e\n", x_plus[i], x[i]);
/* recompute the normal norm */
problem->compute_F(problem->env, n, x_plus, r);
cblas_daxpy(n, -1.0, x, 1, r, 1);
double normal_norm2_newton_point = cblas_ddot(n, r, 1, r, 1);
if (normal_norm2_newton_point > norm_r2)
{
printf("ncp_pathsearch :: lcp successfully solved, but the norm of the normal map increased! %e > %e\n", normal_norm2_newton_point, norm_r2);
//assert(normal_norm2_newton_point <= norm_r2);
}
else
{
printf("ncp_pathsearch :: lcp successfully solved, norm of the normal map decreased! %.*e < %.*e\n", DECIMAL_DIG, normal_norm2_newton_point, DECIMAL_DIG, norm_r2);
}
if (100*normal_norm2_newton_point < norm_r2)
{
force_d_step_merit_check = 1;
}
}
break;
case LCP_PIVOT_NUL:
printf("ncp_pathsearch :: kaboom, kaboom still more work needs to be done\n");
lcp_pivot_diagnose_info(*info);
// exit(EXIT_FAILURE);
force_watchdog_step = 1;
break;
case LCP_PATHSEARCH_NON_ENTERING_T:
DEBUG_PRINT("ncp_pathsearch :: non entering t, something is wrong here. Fix the f****** code!\n");
assert(0 && "ncp_pathsearch :: non entering t, something is wrong here\n");
force_watchdog_step = 1;
break;
default:
printf("ncp_pathsearch :: unknown code returned by the path search\n");
exit(EXIT_FAILURE);
}
nms_failed = NMS(data_NMS, problem, functions, z, x_plus, force_watchdog_step, force_d_step_merit_check, check_ratio);
/* at this point z has been updated */
/* recompute the normal norm */
problem->compute_F(problem->env, n, z, F);
functions->compute_F_merit(problem, z, F, data_NMS->ls_data->F_merit);
/* XXX is this correct ? */
merit_norm = .5 * cblas_ddot(n, data_NMS->ls_data->F_merit, 1, data_NMS->ls_data->F_merit, 1);
ncp_compute_error(n, z, F, nn_tol, &err); /* XXX F should be up-to-date, we should check only CC*/
DEBUG_PRINTF("ncp_pathsearch :: iter = %d, ncp_error = %e; merit_norm^2 = %e\n", nbiter, err, merit_norm);
}
options->iparam[1] = nbiter;
options->dparam[1] = err;
if (nbiter == itermax)
{
*info = 1;
}
else if (nms_failed)
{
*info = 2;
}
else
{
*info = 0;
}
DEBUG_PRINTF("ncp_pathsearch procedure finished :: info = %d; iter = %d; ncp_error = %e; merit_norm^2 = %e\n", *info, nbiter, err, merit_norm);
if (!preAlloc)
{
freeNumericsMatrix(problem->nabla_F);
free(problem->nabla_F);
problem->nabla_F = NULL;
free(options->dWork);
options->dWork = NULL;
solver_options_delete(options->internalSolvers);
free(options->internalSolvers);
options->internalSolvers = NULL;
free_NMS_data(data_NMS);
free(functions);
free(options->solverData);
options->solverData = NULL;
}
}
int main(void)
{
printf("========= Starts Numerics tests for NumericsMatrix ========= \n");
int i, nmm = 4 ;
NumericsMatrix ** NMM = (NumericsMatrix **)malloc(nmm * sizeof(NumericsMatrix *)) ;
NumericsMatrix ** Mread = (NumericsMatrix **)malloc(nmm * sizeof(NumericsMatrix *)) ;
for (i = 0 ; i < nmm; i++)
{
NMM[i] = newNumericsMatrix();
Mread[i] = newNumericsMatrix();
}
int info = test_BuildNumericsMatrix(NMM);
if (info != 0)
{
printf("Construction failed ...\n");
return info;
}
printf("Construction ok ...\n");
/* Test of various I/O functions */
for (i = 0 ; i < nmm; i++)
{
printf("test on NMM[%i]\n", i);
NM_display(NMM[i]);
displayRowbyRow(NMM[i]);
FILE * foutput = fopen("testprintInfile.dat", "w");
printInFile(NMM[i], foutput);
fclose(foutput);
FILE * finput = fopen("testprintInfile.dat", "r");
readInFile(NMM[i], finput);
fclose(finput);
FILE * finput2 = fopen("testprintInfile.dat", "r");
newFromFile(Mread[i], finput2);
fclose(finput2);
char filename[50] = "testprintInfileName.dat";
printInFileName(NMM[i], filename);
readInFileName(NMM[i], filename);
printf("end of test on NMM[%i]\n", i);
}
for (i = 0 ; i < nmm; i++, i++)
{
FILE * foutput2 = fopen("testprintInfileForScilab.dat", "w");
printInFileForScilab(NMM[i], foutput2);
fclose(foutput2);
}
/* free memory */
for (i = 0 ; i < nmm; i++)
{
freeNumericsMatrix(NMM[i]);
free(NMM[i]);
freeNumericsMatrix(Mread[i]);
free(Mread[i]);
}
free(NMM);
free(Mread);
printf("========= End Numerics tests for NumericsMatrix ========= \n");
return info;
}
