/* * options->iWork containing the int work memory. * options->dWork containing the double work memory. * options->iparam[4] : use DGELS (1) or DGESV (0). */ int LinearSystem_driver(LinearSystemProblem* problem, double *z , double *w, SolverOptions* options) { int i; assert(problem->M); if (problem->M->storageType == 1) numericsError("LinearSystem_driver", "forbidden type of storage for the matrix M of the LCP"); #ifdef LINEARSYSTEM_DEBUG displayLS(problem); #endif for (i = 0; i < problem->size; i++) w[i] = 0.0; int res = 0; if (options && options->iparam[4]) { res = solveLeastSquareProblem(problem, z , options); } else { res = myLu(problem, z , options); } if (options && options->filterOn) { options->dparam[1] = LinearSystem_computeError(problem, z); printf("LinearSystem_driver solved with error = %e\n", LinearSystem_computeError(problem, z)); printf("The solution is :\n"); for (i = 0; i < problem->size; i++) printf(" %e", z[i]); printf("\n"); } return res; }
int globalFrictionContact3D_setDefaultSolverOptions(SolverOptions* options, int solverId) { int info = -1; switch (solverId) { case SICONOS_FRICTION_3D_GLOBAL_NSGS: { info = frictionContact3D_nsgs_setDefaultSolverOptions(options); options->solverId = SICONOS_FRICTION_3D_GLOBAL_NSGS; break; } case SICONOS_FRICTION_3D_GLOBAL_LOCALAC_WR: { info = globalFrictionContact3D_globalAlartCurnier_wr_setDefaultSolverOptions(options); break; } case SICONOS_FRICTION_3D_GLOBAL_NSGS_WR: { info = globalFrictionContact3D_nsgs_wr_setDefaultSolverOptions(options); break; } case SICONOS_FRICTION_3D_GLOBAL_NSGSV_WR: { info = globalFrictionContact3D_nsgs_velocity_wr_setDefaultSolverOptions(options); break; } case SICONOS_FRICTION_3D_GLOBAL_PROX_WR: { info = globalFrictionContact3D_proximal_wr_setDefaultSolverOptions(options); break; } case SICONOS_FRICTION_3D_GLOBAL_DSFP_WR: { info = globalFrictionContact3D_DeSaxceFixedPoint_setDefaultSolverOptions(options); break; } case SICONOS_FRICTION_3D_GLOBAL_TFP_WR: { info = globalFrictionContact3D_TrescaFixedPoint_setDefaultSolverOptions(options); break; } case SICONOS_FRICTION_3D_GLOBAL_AC: { info = globalFrictionContact3D_AlartCurnier_setDefaultSolverOptions(options); break; } default: { numericsError("globalFrictionContact3D_setDefaultSolverOptions", "Unknown Solver"); } } return info; }
int myLu(LinearSystemProblem* problem, double *z , SolverOptions* options) { /* Checks inputs */ if (problem == NULL || z == NULL) numericsError("EqualityProblem", "null input for EqualityProblem and/or unknowns (z)"); /* Output info. : 0: ok - >0: problem (depends on solver) */ int info = -1; int n = problem->size; int n2 = n * n; double * Maux = 0; int * ipiv = 0; if (options && options->dWork) Maux = options->dWork; else Maux = (double *) malloc(LinearSystem_getNbDwork(problem, options) * sizeof(double)); if (options && options->iWork) ipiv = options->iWork; else ipiv = (int *) malloc(LinearSystem_getNbIwork(problem, options) * sizeof(int)); int LAinfo = 0; //displayLS(problem); assert(problem->M->matrix0); assert(problem->q); memcpy(Maux, problem->M->matrix0, n2 * sizeof(double)); // memcpy(z,problem->q,n*sizeof(double)); for (int ii = 0; ii < n; ii++) z[ii] = -problem->q[ii]; DGESV(n, 1, Maux, n, ipiv, z, n, &LAinfo); if (!LAinfo) { info = 0; } else { printf("Equality_driver:: LU factorization failed:\n"); } //printf("LinearSystem_driver: computeError of LinearSystem : %e\n",LinearSystemComputeError(problem,z)); if (!(options && options->dWork)) free(Maux); if (!(options && options->iWork)) free(ipiv); return info; }
int soclcp_compute_error_v(SecondOrderConeLinearComplementarityProblem* problem, double *z , double *w, double tolerance, SolverOptions *options, double * error) { /* Checks inputs */ if(problem == NULL || z == NULL || w == NULL) numericsError("soclcp_compute_error", "null input for problem and/or z and/or w"); /* Computes w = Mz + q */ int incx = 1, incy = 1; int nc = problem->nc; int n = problem->n; double *mu = problem->mu; double invmu = 0.0; cblas_dcopy(n , problem->q , incx , z , incy); // z <-q // Compute the current reaction prodNumericsMatrix(n, n, 1.0, problem->M, w, 1.0, z); *error = 0.; double rho = 1.0; for(int ic = 0 ; ic < nc ; ic++) { int dim = problem->coneIndex[ic+1]-problem->coneIndex[ic]; double * worktmp = (double *)malloc(dim*sizeof(double)) ; int nic = problem->coneIndex[ic]; for (int i=0; i < dim; i++) { worktmp[i] = w[nic+i] - rho * z[nic+i]; } invmu = 1.0 / mu[ic]; projectionOnSecondOrderCone(worktmp, invmu, dim); for (int i=0; i < dim; i++) { worktmp[i] = w[nic+i] - worktmp[i]; *error += worktmp[i] * worktmp[i]; } free(worktmp); } *error = sqrt(*error); /* Computes error */ double normq = cblas_dnrm2(n , problem->q , incx); *error = *error / (normq + 1.0); if(*error > tolerance) { /* if (verbose > 0) printf(" Numerics - soclcp_compute_error_velocity failed: error = %g > tolerance = %g.\n",*error, tolerance); */ return 1; } else return 0; }
void AC_fillMLocal(FrictionContactProblem * problem, FrictionContactProblem * localproblem, int contact) { NumericsMatrix * MGlobal = problem->M; int n = 3 * problem->numberOfContacts; // Dense storage int storageType = MGlobal->storageType; if (storageType == 0) { int in = 3 * contact, it = in + 1, is = it + 1; int inc = n * in; double * MM = MGlobal->matrix0; double * MLocal = localproblem->M->matrix0; /* The part of MM which corresponds to the current block is copied into MLocal */ MLocal[0] = MM[inc + in]; MLocal[1] = MM[inc + it]; MLocal[2] = MM[inc + is]; inc += n; MLocal[3] = MM[inc + in]; MLocal[4] = MM[inc + it]; MLocal[5] = MM[inc + is]; inc += n; MLocal[6] = MM[inc + in]; MLocal[7] = MM[inc + it]; MLocal[8] = MM[inc + is]; } else if (storageType == 1) { int diagPos = getDiagonalBlockPos(MGlobal->matrix1, contact); localproblem->M->matrix0 = MGlobal->matrix1->block[diagPos]; /* cblas_dcopy(9, MGlobal->matrix1->block[diagPos], 1, localproblem->M->matrix0 , 1); */ } else numericsError("FrictionContact3D_AlartCurnier:AC_fillMLocal() -", "unknown storage type for matrix M"); }
int variationalInequality_setDefaultSolverOptions(SolverOptions* options, int solverId) { null_SolverOptions(options); int info = -1; switch (solverId) { case SICONOS_VI_EG: { info = variationalInequality_ExtraGradient_setDefaultSolverOptions(options); break; } case SICONOS_VI_FPP: { info = variationalInequality_FixedPointProjection_setDefaultSolverOptions(options); break; } case SICONOS_VI_HP: { info = variationalInequality_HyperplaneProjection_setDefaultSolverOptions(options); break; } case SICONOS_VI_BOX_QI: { info = variationalInequality_common_setDefaultSolverOptions(options, solverId); break; } default: { numericsError("variationalInequality_setDefaultSolverOptions", "Unknown Solver"); } } return info; }
void fc3d_DeSaxceFixedPoint(FrictionContactProblem* problem, double *reaction, double *velocity, int* info, SolverOptions* options) { /* int and double parameters */ int* iparam = options->iparam; double* dparam = options->dparam; /* Number of contacts */ int nc = problem->numberOfContacts; double* q = problem->q; NumericsMatrix* M = problem->M; double* mu = problem->mu; /* Dimension of the problem */ int n = 3 * nc; /* Maximum number of iterations */ int itermax = iparam[0]; /* Tolerance */ double tolerance = dparam[0]; /***** Fixed point 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 */ int nLocal = 3; dparam[0] = dparam[2]; // set the tolerance for the local solver double * velocitytmp = (double *)malloc(n * sizeof(double)); double rho = 0.0; if (dparam[3] > 0.0) { rho = dparam[3]; if (verbose > 0) { printf("----------------------------------- FC3D - DeSaxce Fixed Point (DSFP) - Fixed stepsize with rho = %14.7e \n", rho); } } else { numericsError("fc3d_DeSaxceFixedPoint", "The De Saxce fixed point is implemented with a fixed time--step. Use FixedPointProjection (VI_FPP) method for a variable time--step"); } double alpha = 1.0; double beta = 1.0; while ((iter < itermax) && (hasNotConverged > 0)) { ++iter; /* velocitytmp <- q */ cblas_dcopy(n , q , 1 , velocitytmp, 1); /* velocitytmp <- q + M * reaction */ beta = 1.0; prodNumericsMatrix(n, n, alpha, M, reaction, beta, velocitytmp); /* projection for each contact */ for (contact = 0 ; contact < nc ; ++contact) { int pos = contact * nLocal; double normUT = sqrt(velocitytmp[pos + 1] * velocitytmp[pos + 1] + velocitytmp[pos + 2] * velocitytmp[pos + 2]); reaction[pos] -= rho * (velocitytmp[pos] + mu[contact] * normUT); reaction[pos + 1] -= rho * velocitytmp[pos + 1]; reaction[pos + 2] -= rho * velocitytmp[pos + 2]; projectionOnCone(&reaction[pos], mu[contact]); } /* **** Criterium convergence **** */ fc3d_compute_error(problem, reaction , velocity, tolerance, options, &error); if (options->callback) { options->callback->collectStatsIteration(options->callback->env, nc * 3, reaction, velocity, error, NULL); } if (verbose > 0) printf("----------------------------------- FC3D - DeSaxce Fixed Point (DSFP) - Iteration %i rho = %14.7e \tError = %14.7e\n", iter, rho, error); if (error < tolerance) hasNotConverged = 0; *info = hasNotConverged; } if (verbose > 0) printf("----------------------------------- FC3D - DeSaxce Fixed point (DSFP) - #Iteration %i Final Residual = %14.7e\n", iter, error); iparam[7] = iter; dparam[0] = tolerance; dparam[1] = error; free(velocitytmp); }
int fc3d_driver(FrictionContactProblem* problem, double *reaction, double *velocity, SolverOptions* options, NumericsOptions* global_options) { if (options == NULL) numericsError("fc3d_driver", "null input for solver and/or global options"); int setnumericsoptions=0; /* Set global options */ if (global_options) { setNumericsOptions(global_options); options->numericsOptions = (NumericsOptions*) malloc(sizeof(NumericsOptions)); options->numericsOptions->verboseMode = global_options->verboseMode; setnumericsoptions=1; } int NoDefaultOptions = options->isSet; /* true(1) if the SolverOptions structure has been filled in else false(0) */ if (!NoDefaultOptions) readSolverOptions(3, options); if (verbose > 0) printSolverOptions(options); /* Solver name */ /*char * name = options->solverName;*/ int info = -1 ; if (problem->dimension != 3) numericsError("fc3d_driver", "Dimension of the problem : problem-> dimension is not compatible or is not set"); /* Check for trivial case */ info = checkTrivialCase(problem, velocity, reaction, options); if (info == 0) goto exit; switch (options->solverId) { /* Non Smooth Gauss Seidel (NSGS) */ case SICONOS_FRICTION_3D_NSGS: { snPrintf(1, options, " ========================== Call NSGS solver for Friction-Contact 3D problem ==========================\n"); fc3d_nsgs(problem, reaction , velocity , &info , options); break; } case SICONOS_FRICTION_3D_NSGSV: { snPrintf(1, options, " ========================== Call NSGSV solver for Friction-Contact 3D problem ==========================\n"); fc3d_nsgs_velocity(problem, reaction , velocity , &info , options); break; } /* Proximal point algorithm */ case SICONOS_FRICTION_3D_PROX: { snPrintf(1, options, " ========================== Call PROX (Proximal Point) solver for Friction-Contact 3D problem ==========================\n"); fc3d_proximal(problem, reaction , velocity , &info , options); break; } /* Tresca Fixed point algorithm */ case SICONOS_FRICTION_3D_TFP: { snPrintf(1, options, " ========================== Call TFP (Tresca Fixed Point) solver for Friction-Contact 3D problem ==========================\n"); fc3d_TrescaFixedPoint(problem, reaction , velocity , &info , options); break; } /* ACLM Fixed point algorithm */ case SICONOS_FRICTION_3D_ACLMFP: { snPrintf(1, options, " ========================== Call ACLM (Acary Cadoux Lemarechal Malick Fixed Point) solver for Friction-Contact 3D problem ==========================\n"); fc3d_ACLMFixedPoint(problem, reaction , velocity , &info , options); break; } /* SOCLCP Fixed point algorithm */ case SICONOS_FRICTION_3D_SOCLCP: { snPrintf(1, options, " ========================== Call SOCLCP solver for Friction-Contact 3D problem (Associated one) ==========================\n"); fc3d_SOCLCP(problem, reaction , velocity , &info , options); break; } /* De Saxce Fixed point algorithm */ case SICONOS_FRICTION_3D_DSFP: { snPrintf(1, options, " ========================== Call DeSaxce Fixed Point (DSFP) solver for Friction-Contact 3D problem ==========================\n"); fc3d_DeSaxceFixedPoint(problem, reaction , velocity , &info , options); break; } /* Fixed point projection algorithm */ case SICONOS_FRICTION_3D_FPP: { snPrintf(1, options, " ========================== Call Fixed Point Projection (FPP) solver for Friction-Contact 3D problem ==========================\n"); fc3d_fixedPointProjection(problem, reaction , velocity , &info , options); break; } /* Extra Gradient algorithm */ case SICONOS_FRICTION_3D_EG: { snPrintf(1, options, " ========================== Call ExtraGradient (EG) solver for Friction-Contact 3D problem ==========================\n"); fc3d_ExtraGradient(problem, reaction , velocity , &info , options); break; } /* VI Fixed Point Projection algorithm */ case SICONOS_FRICTION_3D_VI_FPP: { snPrintf(1, options, " ========================== Call VI_FixedPointProjection (VI_FPP) solver for Friction-Contact 3D problem ==========================\n"); fc3d_VI_FixedPointProjection(problem, reaction , velocity , &info , options); break; } /* VI Extra Gradient algorithm */ case SICONOS_FRICTION_3D_VI_EG: { snPrintf(1, options, " ========================== Call VI_ExtraGradient (VI_EG) solver for Friction-Contact 3D problem ==========================\n"); fc3d_VI_ExtraGradient(problem, reaction , velocity , &info , options); break; } /* Hyperplane Projection algorithm */ case SICONOS_FRICTION_3D_HP: { snPrintf(1, options, " ========================== Call Hyperplane Projection (HP) solver for Friction-Contact 3D problem ==========================\n"); fc3d_HyperplaneProjection(problem, reaction , velocity , &info , options); break; } /* Alart Curnier in local coordinates */ case SICONOS_FRICTION_3D_NSN_AC: { snPrintf(1, options, " ========================== Call Alart Curnier solver for Friction-Contact 3D problem ==========================\n"); if (problem->M->matrix0) { fc3d_nonsmooth_Newton_AlartCurnier(problem, reaction , velocity , &info , options); } else { fc3d_nonsmooth_Newton_AlartCurnier(problem, reaction , velocity , &info , options); } break; } /* Fischer Burmeister in local coordinates */ case SICONOS_FRICTION_3D_NSN_FB: { snPrintf(1, options, " ========================== Call Fischer Burmeister solver for Friction-Contact 3D problem ==========================\n"); fc3d_nonsmooth_Newton_FischerBurmeister(problem, reaction , velocity , &info , options); break; } case SICONOS_FRICTION_3D_NSN_NM: { snPrintf(1, options, " ========================== Call natural map solver for Friction-Contact 3D problem ==========================\n"); fc3d_nonsmooth_Newton_NaturalMap(problem, reaction , velocity , &info , options); break; } case SICONOS_FRICTION_3D_ONECONTACT_QUARTIC_NU: case SICONOS_FRICTION_3D_ONECONTACT_QUARTIC: { snPrintf(1, options, " ========================== Call Quartic solver for Friction-Contact 3D problem ==========================\n"); fc3d_unitary_enumerative(problem, reaction , velocity , &info , options); break; } case SICONOS_FRICTION_3D_ONECONTACT_NSN_AC: case SICONOS_FRICTION_3D_ONECONTACT_NSN_AC_GP: { snPrintf(1, options, " ========================== Call Newton-based solver for one contact Friction-Contact 3D problem ==========================\n"); fc3d_onecontact_nonsmooth_Newton_solvers_initialize(problem, problem, options); info = fc3d_onecontact_nonsmooth_Newton_solvers_solve(problem, reaction , options); break; } case SICONOS_FRICTION_3D_ONECONTACT_ProjectionOnConeWithLocalIteration: { snPrintf(1, options, " ========================== Call Projection on cone solver for one contact Friction-Contact 3D problem ==========================\n"); fc3d_projectionOnConeWithLocalIteration_initialize(problem, problem, options); info = fc3d_projectionOnConeWithLocalIteration_solve(problem, reaction , options); fc3d_projectionOnConeWithLocalIteration_free(problem, problem, options); break; } case SICONOS_FRICTION_3D_GAMS_PATH: { snPrintf(1, options, " ========================== Call PATH solver via GAMS for an AVI Friction-Contact 3D problem ==========================\n"); fc3d_AVI_gams_path(problem, reaction , velocity, &info, options); break; } case SICONOS_FRICTION_3D_GAMS_PATHVI: { snPrintf(1, options, " ========================== Call PATHVI solver via GAMS for an AVI Friction-Contact 3D problem ==========================\n"); fc3d_AVI_gams_pathvi(problem, reaction , velocity, &info, options); break; } case SICONOS_FRICTION_3D_GAMS_LCP_PATH: { snPrintf(1, options, " ========================== Call PATH solver via GAMS for an LCP-based reformulation of the AVI Friction-Contact 3D problem ==========================\n"); fc3d_lcp_gams_path(problem, reaction , velocity, &info, options); break; } case SICONOS_FRICTION_3D_GAMS_LCP_PATHVI: { snPrintf(1, options, " ========================== Call PATHVI solver via GAMS for an LCP-based reformulation of the AVI Friction-Contact 3D problem ==========================\n"); fc3d_lcp_gams_pathvi(problem, reaction , velocity, &info, options); break; } default: { fprintf(stderr, "Numerics, fc3d_driver failed. Unknown solver.\n"); exit(EXIT_FAILURE); } } exit: if (setnumericsoptions) { free(options->numericsOptions); options->numericsOptions = NULL; } return info; }
int soclcp_setDefaultSolverOptions(SolverOptions* options, int solverId) { options->iparam = NULL; options->dparam = NULL; null_SolverOptions(options); int info = -1; switch(solverId) { case SICONOS_SOCLCP_NSGS: { info = soclcp_nsgs_setDefaultSolverOptions(options); break; } /* case SICONOS_SOCLCP_NSGSV: */ /* { */ /* info = soclcp_nsgs_velocity_setDefaultSolverOptions(options); */ /* break; */ /* } */ /* case SICONOS_SOCLCP_*/ /* { */ /* info = soclcp_proximal_setDefaultSolverOptions(options); */ /* break; */ /* } */ /* case SICONOS_SOCLCP_TFP: */ /* { */ /* info = soclcp_TrescaFixedPoint_setDefaultSolverOptions(options); */ /* break; */ /* } */ /* case SICONOS_SOCLCP_DSFP: */ /* { */ /* info = soclcp_DeSaxceFixedPoint_setDefaultSolverOptions(options); */ /* break; */ /* } */ /* case SICONOS_SOCLCP_FPP: */ /* { */ /* info = soclcp_fixedPointProjection_setDefaultSolverOptions(options); */ /* break; */ /* } */ /* case SICONOS_SOCLCP_EG: */ /* { */ /* info = soclcp_ExtraGradient_setDefaultSolverOptions(options); */ /* break; */ /* } */ case SICONOS_SOCLCP_VI_FPP: { info = soclcp_VI_FixedPointProjection_setDefaultSolverOptions(options); break; } case SICONOS_SOCLCP_VI_EG: { info = soclcp_VI_ExtraGradient_setDefaultSolverOptions(options); break; } /* case SICONOS_SOCLCP_HP: */ /* { */ /* info = soclcp_HyperplaneProjection_setDefaultSolverOptions(options); */ /* break; */ /* } */ /* case SICONOS_SOCLCP_LOCALAC: */ /* { */ /* info = soclcp_AlartCurnier_setDefaultSolverOptions(options); */ /* break; */ /* } */ /* case SICONOS_SOCLCP_LOCALFB: */ /* { */ /* info = soclcp_FischerBurmeister_setDefaultSolverOptions(options); */ /* break; */ /* } */ /* case SICONOS_SOCLCP_QUARTIC: */ /* { */ /* info = soclcp_unitary_enumerative_setDefaultSolverOptions(options); */ /* break; */ /* } */ /* case SICONOS_SOCLCP_QUARTIC_NU: */ /* { */ /* info = soclcp_unitary_enumerative_setDefaultSolverOptions(options); */ /* options->solverId = SICONOS_SOCLCP_QUARTIC_NU; */ /* break; */ /* } */ default: { numericsError("soclcp_setDefaultSolverOptions", "Unknown Solver"); } } return info; }
int soclcp_driver(SecondOrderConeLinearComplementarityProblem* problem, double *r, double *v, SolverOptions* options, NumericsOptions* global_options) { if(options == NULL) numericsError("soclcp_driver", "null input for solver and/or global options"); int setnumericsoptions=0; /* Set global options */ if(global_options) { setNumericsOptions(global_options); options->numericsOptions = (NumericsOptions*) malloc(sizeof(NumericsOptions)); options->numericsOptions->verboseMode = global_options->verboseMode; setnumericsoptions=1; } int NoDefaultOptions = options->isSet; /* true(1) if the SolverOptions structure has been filled in else false(0) */ if(!NoDefaultOptions) readSolverOptions(3, options); if(verbose > 0) printSolverOptions(options); /* Solver name */ /*char * name = options->solverName;*/ int info = -1 ; /* Check for trivial case */ info = soclcp_checkTrivialCase(problem, v, r, options); if(info == 0) return info; switch(options->solverId) { /* Non Smooth Gauss Seidel (NSGS) */ case SICONOS_SOCLCP_NSGS: { snPrintf(1, options, " ========================== Call NSGS solver for Second Order Cone LCP problem ==========================\n"); soclcp_nsgs(problem, r , v , &info , options); break; } /* case SICONOS_SOCLCP_NSGSV: */ /* { */ /* snPrintf(1, options, */ /* " ========================== Call NSGSV solver for Second Order Cone LCP problem ==========================\n"); */ /* soclcp_nsgs_v(problem, r , v , &info , options); */ /* break; */ /* } */ /* /\* Proximal point algorithm *\/ */ /* case SICONOS_SOCLCP_PROX: */ /* { */ /* snPrintf(1, options, */ /* " ========================== Call PROX (Proximal Point) solver for Second Order Cone LCP problem ==========================\n"); */ /* soclcp_proximal(problem, r , v , &info , options); */ /* break; */ /* } */ /* /\* Tresca Fixed point algorithm *\/ */ /* case SICONOS_SOCLCP_TFP: */ /* { */ /* snPrintf(1, options, */ /* " ========================== Call TFP (Tresca Fixed Point) solver for Second Order Cone LCP problem ==========================\n"); */ /* soclcp_TrescaFixedPoint(problem, r , v , &info , options); */ /* break; */ /* } */ case SICONOS_SOCLCP_VI_FPP: { snPrintf(1, options, " ========================== Call VI_FixedPointProjection (VI_FPP) solver for Second Order Cone LCP problem ==========================\n"); soclcp_VI_FixedPointProjection(problem, r , v , &info , options); break; } /* VI Extra Gradient algorithm */ case SICONOS_SOCLCP_VI_EG: { snPrintf(1, options, " ========================== Call VI_ExtraGradient (VI_EG) solver for Second Order Cone LCP problem ==========================\n"); soclcp_VI_ExtraGradient(problem, r , v , &info , options); break; } /* /\* Hyperplane Projection algorithm *\/ */ /* case SICONOS_SOCLCP_HP: */ /* { */ /* snPrintf(1, options, */ /* " ========================== Call Hyperplane Projection (HP) solver for Second Order Cone LCP problem ==========================\n"); */ /* soclcp_HyperplaneProjection(problem, r , v , &info , options); */ /* break; */ /* } */ /* /\* Alart Curnier in local coordinates *\/ */ /* case SICONOS_SOCLCP_LOCALAC: */ /* { */ /* snPrintf(1, options, */ /* " ========================== Call Alart Curnier solver for Second Order Cone LCP problem ==========================\n"); */ /* if (problem->M->matrix0) */ /* { */ /* soclcp_localAlartCurnier(problem, r , v , &info , options); */ /* } */ /* else */ /* { */ /* soclcp_localAlartCurnier(problem, r , v , &info , options); */ /* } */ /* break; */ /* } */ /* /\* Fischer Burmeister in local coordinates *\/ */ /* case SICONOS_SOCLCP_LOCALFB: */ /* { */ /* snPrintf(1, options, */ /* " ========================== Call Fischer Burmeister solver for Second Order Cone LCP problem ==========================\n"); */ /* soclcp_localFischerBurmeister(problem, r , v , &info , options); */ /* break; */ /* } */ /* case SICONOS_SOCLCP_QUARTIC_NU: */ /* case SICONOS_SOCLCP_QUARTIC: */ /* { */ /* snPrintf(1, options, */ /* " ========================== Call Quartic solver for Second Order Cone LCP problem ==========================\n"); */ /* soclcp_unitary_enumerative(problem, r , v , &info , options); */ /* break; */ /* } */ /* case SICONOS_SOCLCP_AlartCurnierNewton: */ /* case SICONOS_SOCLCP_DampedAlartCurnierNewton: */ /* { */ /* snPrintf(1, options, */ /* " ========================== Call Quartic solver for Second Order Cone LCP problem ==========================\n"); */ /* info =soclcp_Newton_solve(problem, r , options); */ /* break; */ /* } */ default: { fprintf(stderr, "Numerics, SecondOrderConeLinearComplementarity_driver failed. Unknown solver.\n"); exit(EXIT_FAILURE); } } if(setnumericsoptions) { free(options->numericsOptions); options->numericsOptions = NULL; } return info; }
int fc2d_tolcp(FrictionContactProblem* problem, LinearComplementarityProblem * lcp_problem) { if (problem->dimension != 2) { numericsError("fc2d_tolcp", "Dimension of the problem : problem-> dimension is not compatible or is not set"); return 1; } int nc = problem->numberOfContacts; lcp_problem->size = 3 * nc ; lcp_problem->M = newNumericsMatrix(); lcp_problem->M->size0 = 3 * nc ; lcp_problem->M->size1 = 3 * nc ; lcp_problem->M->storageType = 0; lcp_problem->M->matrix1 = NULL; lcp_problem->M->matrix2 = NULL; lcp_problem->M->internalData = NULL; lcp_problem->M->matrix0 = (double*)malloc(lcp_problem->size * lcp_problem->size * sizeof(double));; lcp_problem->q = (double*)malloc(lcp_problem->size * sizeof(double)); int n = 2 * nc; int i, j; for (i = 0; i < nc; i++) { for (j = 0; j < nc; j++) { /* first Column */ lcp_problem->M->matrix0[3 * i + 3 * j * lcp_problem->size] = problem->M->matrix0[2 * i + 2 * j * n] - problem->mu[i] * problem->M->matrix0[2 * i + (2 * j + 1) * n] ; // compute W_NN-mu W_NT lcp_problem->M->matrix0[3 * i + 1 + 3 * j * lcp_problem->size] = problem->M->matrix0[(2 * i + 1) + 2 * j * n] - problem->mu[i] * problem->M->matrix0[(2 * i + 1) + (2 * j + 1) * n] ; // compute W_TN-mu W_TT if (i == j) { lcp_problem->M->matrix0[3 * i + 2 + 3 * j * lcp_problem->size] = 2.0 * problem->mu[i]; } else { lcp_problem->M->matrix0[3 * i + 2 + 3 * j * lcp_problem->size] = 0.0; } /* second Column */ lcp_problem->M->matrix0[3 * i + (3 * j + 1)*lcp_problem->size] = problem->M->matrix0[2 * i + (2 * j + 1) * n] ; // set W_NT lcp_problem->M->matrix0[3 * i + 1 + (3 * j + 1)*lcp_problem->size] = problem->M->matrix0[(2 * i + 1) + (2 * j + 1) * n] ; // set WTT if (i == j) { lcp_problem->M->matrix0[3 * i + 2 + (3 * j + 1)*lcp_problem->size] = -1.0; } else { lcp_problem->M->matrix0[3 * i + 2 + (3 * j + 1)*lcp_problem->size] = 0.0; } /* Third Column */ lcp_problem->M->matrix0[3 * i + (3 * j + 2)*lcp_problem->size] = 0.0; if (i == j) { lcp_problem->M->matrix0[3 * i + 1 + (3 * j + 2)*lcp_problem->size] = 1.0; } else { lcp_problem->M->matrix0[3 * i + 1 + (3 * j + 2)*lcp_problem->size] = 0.0; } lcp_problem->M->matrix0[3 * i + 2 + (3 * j + 2)*lcp_problem->size] = 0.0; } } for (i = 0; i < nc; i++) { lcp_problem->q[3 * i ] = problem->q[2 * i]; lcp_problem->q[3 * i + 1] = problem->q[2 * i + 1]; lcp_problem->q[3 * i + 2] = 0.0;; } return 0; }
void fc3d_ACLMFixedPoint(FrictionContactProblem* problem, double *reaction, double *velocity, int* info, SolverOptions* options) { /* int and double parameters */ int* iparam = options->iparam; double* dparam = options->dparam; /* Number of contacts */ int nc = problem->numberOfContacts; /* Maximum number of iterations */ int itermax = iparam[0]; /* Tolerance */ double tolerance = dparam[0]; if (options->numberOfInternalSolvers < 1) { numericsError("fc3d_ACLMFixedpoint", "The ACLM Fixed Point method needs options for the internal solvers, options[0].numberOfInternalSolvers should be >1"); } SolverOptions * internalsolver_options = options->internalSolvers; if (verbose > 0) { printSolverOptions(options); } /***** Fixed Point Iterations *****/ int iter = 0; /* Current iteration number */ double error = 1.; /* Current error */ int hasNotConverged = 1; soclcp_InternalSolverPtr internalsolver; SecondOrderConeLinearComplementarityProblem* soclcp = (SecondOrderConeLinearComplementarityProblem *)malloc(sizeof(SecondOrderConeLinearComplementarityProblem)); soclcp->n = problem->numberOfContacts * problem->dimension; soclcp->nc= problem->numberOfContacts; soclcp->M = problem-> M; soclcp->q = (double *) malloc(soclcp->n * sizeof(double)); soclcp->mu = problem->mu; soclcp->coneIndex = (unsigned int *) malloc((soclcp->nc+1) * sizeof(unsigned int)); for (int i=0; i < soclcp->n; i++) { soclcp->q[i]=problem->q[i]; } for (int i=0; i < soclcp->nc+1; i++) { soclcp->coneIndex[i] = 3*i; } if (internalsolver_options->solverId == SICONOS_SOCLCP_NSGS) { if (verbose == 1) printf(" ========================== Call NSGS solver SOCLCP problem ==========================\n"); internalsolver = &soclcp_nsgs; //internalsolver_options->internalSolvers->dWork = options->dWork; } else { fprintf(stderr, "Numerics, fc3d_ACLMFixedPoint failed. Unknown internal solver.\n"); exit(EXIT_FAILURE); } double normUT; int cumul_iter =0; while ((iter < itermax) && (hasNotConverged > 0)) { ++iter; // internal solver for the regularized problem /* Compute the value of the initial value of q */ for (int ic = 0 ; ic < nc ; ic++) { normUT = sqrt(velocity[ic*3+1] * velocity[ic*3+1] + velocity[ic*3+2] * velocity[ic*3+2]); soclcp->q[3*ic] = problem->q[3*ic] + problem->mu[ic]*normUT; } //secondOrderConeLinearComplementarityProblem_printInFilename(soclcp,"output.dat"); // DEBUG_EXPR(for (int ic = 0 ; ic < nc ; ic++) printf("problem->q[%i] = %le\n", 3*ic, problem->q[3*ic]); // for (int ic = 0 ; ic < nc ; ic++) printf("q[%i] = %le\n", 3*ic, soclcp->q[3*ic]); ); if (iparam[1] == 0 ) { internalsolver_options->dparam[0] = max(error/10.0, options->dparam[0]/problem->numberOfContacts); } else if (iparam[1] ==1) { internalsolver_options->dparam[0] = options->dparam[0]/2.0; } else { fprintf(stderr, "Numerics, fc3d_ACLMFixedPoint failed. Unknown startegy for driving tolerence of internal.\n"); exit(EXIT_FAILURE); } (*internalsolver)(soclcp, reaction , velocity , info , internalsolver_options); cumul_iter += internalsolver_options->iparam[7]; /* **** Criterium convergence **** */ fc3d_compute_error(problem, reaction , velocity, tolerance, options, &error); if (options->callback) { options->callback->collectStatsIteration(options->callback->env, nc * 3, reaction, velocity, error, NULL); } if (verbose > 0) printf("------------------------ FC3D - ACLMFP - Iteration %i Residual = %14.7e\n", iter, error); if (error < tolerance) hasNotConverged = 0; *info = hasNotConverged; } if (verbose > 0) { printf("----------------------------------- FC3D - ACLMFP - # Iteration %i Final Residual = %14.7e\n", iter, error); printf("----------------------------------- FC3D - ACLMFP - # internal iteration = %i\n", cumul_iter); } free(soclcp->q); free(soclcp->coneIndex); free(soclcp); if (internalsolver_options->internalSolvers != NULL) internalsolver_options->internalSolvers->dWork = NULL; dparam[0] = tolerance; dparam[1] = error; iparam[7] = iter; }
int fc2d_driver(FrictionContactProblem* problem, double *reaction , double *velocity, SolverOptions* options, NumericsOptions* global_options) { #ifdef DUMP_PROBLEM char fname[256]; sprintf(fname, "FrictionContactProblem%.5d.dat", fccounter++); printf("Dump of FrictionContactProblem%.5d.dat", fccounter); FILE * foutput = fopen(fname, "w"); frictionContact_printInFile(problem, foutput); fclose(foutput); #endif if (options == NULL || global_options == NULL) numericsError("fc2d_driver", "null input for solver and/or global options"); /* Set global options */ setNumericsOptions(global_options); /* Checks inputs */ if (problem == NULL || reaction == NULL || velocity == NULL) numericsError("fc2d_driver", "null input for FrictionContactProblem and/or unknowns (reaction,velocity)"); /* If the options for solver have not been set, read default values in .opt file */ int NoDefaultOptions = options->isSet; /* true(1) if the SolverOptions structure has been filled in else false(0) */ if (!NoDefaultOptions) readSolverOptions(3, options); if (verbose > 0) printSolverOptions(options); /* Solver name */ /*char * name = options->solverName;*/ int info = -1 ; if (problem->dimension != 2) numericsError("fc2d_driver", "Dimension of the problem : problem-> dimension is not compatible or is not set"); /* Non Smooth Gauss Seidel (NSGS) */ if (problem->M->storageType == 1) { if (options->solverId == SICONOS_FRICTION_2D_NSGS) { if (verbose) printf(" ======================= Call Sparse NSGS solver for Friction-Contact 2D problem ======================\n"); fc2d_sparse_nsgs(problem, reaction , velocity , &info , options); } else { fprintf(stderr, "fc2d_driver error: unknown solver named: %s\n", idToName(options->solverId)); exit(EXIT_FAILURE); } } else if (problem->M->storageType == 0) { switch (options->solverId) { /****** NLGS algorithm ******/ case SICONOS_FRICTION_2D_PGS: case SICONOS_FRICTION_2D_NSGS: { if (verbose) printf(" ========================== Call NLGS solver for Friction-Contact 2D problem ==========================\n"); fc2d_nsgs(problem, reaction, velocity, &info, options); break; } /****** CPG algorithm ******/ case SICONOS_FRICTION_2D_CPG: { if (verbose) printf(" ========================== Call CPG solver for Friction-Contact 2D problem ==========================\n"); fc2d_cpg(problem, reaction, velocity, &info, options); break; } /****** Latin algorithm ******/ case SICONOS_FRICTION_2D_LATIN: { if (verbose) printf(" ========================== Call Latin solver for Friction-Contact 2D problem ==========================\n"); fc2d_latin(problem, reaction, velocity, &info, options); break; } /****** Lexicolemke algorithm ******/ case SICONOS_FRICTION_2D_LEMKE: { if (verbose) printf(" ========================== Call Lemke solver for Friction-Contact 2D problem ==========================\n"); fc2d_lexicolemke(problem, reaction, velocity, &info, options, global_options); break; } /****** Enum algorithm ******/ case SICONOS_FRICTION_2D_ENUM: { if (verbose) printf(" ========================== Call Enumerative solver for Friction-Contact 2D problem ==========================\n"); fc2d_enum(problem, reaction, velocity, &info, options, global_options); break; } /*error */ default: { fprintf(stderr, "fc2d_driver error: unknown solver named: %s\n", idToName(options->solverId)); exit(EXIT_FAILURE); } } #ifdef DUMP_PROBLEM_IF_INFO if (info) { char fname[256]; sprintf(fname, "FrictionContactProblem%.5d.dat", fccounter++); printf("Dump of FrictionContactProblem%.5d.dat\n", fccounter); FILE * foutput = fopen(fname, "w"); frictionContact_printInFile(problem, foutput); fclose(foutput); } #endif } else { numericsError("fc2d_driver", " error: unknown storagetype named"); exit(EXIT_FAILURE); } return info; }
/* return 0 if ok * otherwise return !=0 */ int solveLeastSquareProblem(LinearSystemProblem* problem, double *z , SolverOptions* options) { /* Checks inputs */ if (problem == NULL || z == NULL) numericsError("EqualityProblem", "null input for EqualityProblem and/or unknowns (z)"); /* Output info. : 0: ok - >0: problem (depends on solver) */ int info = -1; int n = problem->size; int n2 = n * n; double * Maux = 0; int * ipiv = 0; if (options && options->dWork) Maux = options->dWork; else Maux = (double *) malloc(LinearSystem_getNbDwork(problem, options) * sizeof(double)); if (options && options->iWork) ipiv = options->iWork; else ipiv = (int *) malloc(LinearSystem_getNbIwork(problem, options) * sizeof(int)); int LAinfo = 0; //displayLS(problem); assert(problem->M->matrix0); assert(problem->q); memcpy(Maux, problem->M->matrix0, n2 * sizeof(double)); // memcpy(z,problem->q,n*sizeof(double)); for (int ii = 0; ii < n; ii++) z[ii] = -problem->q[ii]; printf("LinearSystem : solveLeastSquareProblem LWORK is :%d\n", LWORK); DGELS(LA_NOTRANS,n, n, 1, Maux, n, z, n, &LAinfo); if (LAinfo) { printf("LinearSystem_driver: DGELS failed:\n"); goto __fin; } int ii; for (ii = 0; ii < n; ii++) { if (isnan(z[ii]) || isinf(z[ii])) { printf("DGELS FAILED\n"); goto __fin; } } info = 0; printf("LinearSystem_driver: computeError of LinearSystem : %e\n", LinearSystem_computeError(problem, z)); __fin: if (!(options && options->dWork)) free(Maux); if (!(options && options->iWork)) free(ipiv); return info; }
void frictionContact3D_projection_update_with_regularization(int contact, FrictionContactProblem * problem, FrictionContactProblem * localproblem, double* reaction, SolverOptions* options) { /* Build a local problem for a specific contact reaction corresponds to the global vector (size n) of the global problem. */ /* Call the update function which depends on the storage for MGlobal/MBGlobal */ /* Build a local problem for a specific contact reaction corresponds to the global vector (size n) of the global problem. */ /* The part of MGlobal which corresponds to the current block is copied into MLocal */ NumericsMatrix * MGlobal = problem->M; int n = 3 * problem->numberOfContacts; int storageType = MGlobal->storageType; if (storageType == 0) // Dense storage { int in = 3 * contact, it = in + 1, is = it + 1; int inc = n * in; double * MM = MGlobal->matrix0; double * MLocal = localproblem->M->matrix0; /* The part of MM which corresponds to the current block is copied into MLocal */ MLocal[0] = MM[inc + in]; MLocal[1] = MM[inc + it]; MLocal[2] = MM[inc + is]; inc += n; MLocal[3] = MM[inc + in]; MLocal[4] = MM[inc + it]; MLocal[5] = MM[inc + is]; inc += n; MLocal[6] = MM[inc + in]; MLocal[7] = MM[inc + it]; MLocal[8] = MM[inc + is]; } else if (storageType == 1) { int diagPos = getDiagonalBlockPos(MGlobal->matrix1, contact); /* for (int i =0 ; i< 3*3 ; i++) localproblem->M->matrix0[i] = MGlobal->matrix1->block[diagPos][i] ; */ cblas_dcopy(9, MGlobal->matrix1->block[diagPos], 1, localproblem->M->matrix0 , 1); } else numericsError("FrictionContact3D_projection -", "unknown storage type for matrix M"); /**** Computation of qLocal = qBlock + sum over a row of blocks in MGlobal of the products MLocal.reactionBlock, excluding the block corresponding to the current contact. ****/ frictionContact3D_nsgs_computeqLocal(problem, localproblem, reaction, contact); double rho = options->dparam[3]; for (int i = 0 ; i < 3 ; i++) localproblem->M->matrix0[i + 3 * i] += rho ; double *qLocal = localproblem->q; int in = 3 * contact, it = in + 1, is = it + 1; /* qLocal computation*/ qLocal[0] -= rho * reaction[in]; qLocal[1] -= rho * reaction[it]; qLocal[2] -= rho * reaction[is]; /* Friction coefficient for current block*/ localproblem->mu[0] = problem->mu[contact]; }
void lcp_nsgs_SBM(LinearComplementarityProblem* problem, double *z, double *w, int *info, SolverOptions* options) { /* Notes: - we suppose that the trivial solution case has been checked before, and that all inputs differs from NULL since this function is supposed to be called from lcp_driver_global(). - Input matrix M of the problem is supposed to be sparse-block with no null row (ie no rows with all blocks equal to null) */ if (problem->M->matrix1 == NULL) { fprintf(stderr, "lcp_NSGS_SBM error: wrong storage type for input matrix M of the LCP.\n"); exit(EXIT_FAILURE); } /* The options for the global "block" solver are defined in options[0].\n options[i], for 0<i<numberOfSolvers-1 correspond to local solvers. */ /* Global Solver parameters*/ int itermax = options[0].iparam[0]; double tolerance = options[0].dparam[0]; /* Matrix M/vector q of the LCP */ SparseBlockStructuredMatrix* blmat = problem->M->matrix1; double * q = problem->q; /* Number of non-null blocks in blmat */ int nbOfNonNullBlocks = blmat->nbblocks; if (nbOfNonNullBlocks < 1) { fprintf(stderr, "Numerics::lcp_NSGS_SBM error: empty M matrix (all blocks = NULL).\n"); exit(EXIT_FAILURE); } /* Local problem initialization */ LinearComplementarityProblem * local_problem = (LinearComplementarityProblem *)malloc(sizeof(*local_problem)); local_problem->M = (NumericsMatrix *)malloc(sizeof(*local_problem->M)); local_problem->M->storageType = 0; // dense storage local_problem->M->matrix0 = NULL; local_problem->M->matrix1 = NULL; local_problem->M->matrix2 = NULL; local_problem->M->internalData = NULL; /* Memory allocation for q. Size of q = blsizemax, size of the largest square-block in blmat */ int blsizemax = blmat->blocksize0[0]; int k; for (unsigned int i = 1 ; i < blmat->blocknumber0 ; i++) { k = blmat->blocksize0[i] - blmat->blocksize0[i - 1]; if (k > blsizemax) blsizemax = k; } local_problem->q = (double*)malloc(blsizemax * sizeof(double)); /* Current row (of blocks) number */ unsigned int rowNumber; /***** Gauss-Seidel iterations *****/ int iter = 0; /* Current iteration number */ double error = 1.; /* Current error */ int hasNotConverged = 1; /* Output from local solver */ options[0].iparam[2] = 0; options[0].dparam[2] = 0.0; if (options->numberOfInternalSolvers < 1) { numericsError("lcp_nsgs_SBM", "The NSGS_SBM method needs options for the internal solvers, options[0].numberOfInternalSolvers should be >1"); } /*Number of the local solver */ int localSolverNum = options->numberOfInternalSolvers ; SolverOptions * internalSolvers = options->internalSolvers ; int pos = 0; /* Output from local solver */ int infoLocal = -1; while ((iter < itermax) && (hasNotConverged > 0)) { ++iter; /* Loop over the rows of blocks in blmat */ localSolverNum = 0; pos = 0; /* cblas_dcopy(problem->size,w,1,wBackup,1); */ for (rowNumber = 0; rowNumber < blmat->blocknumber0; ++rowNumber) { /* Local problem formalization */ lcp_nsgs_SBM_buildLocalProblem(rowNumber, blmat, local_problem, q, z); /* Solve local problem */ infoLocal = lcp_driver_DenseMatrix(local_problem, &z[pos], &w[pos], &internalSolvers[localSolverNum]); pos += local_problem->size; /* sum of local number of iterations (output from local_driver)*/ if (options[localSolverNum].iparam != NULL) options[0].iparam[2] += internalSolvers[localSolverNum].iparam[1]; /* sum of local errors (output from local_driver)*/ options[0].dparam[2] += internalSolvers[localSolverNum].dparam[1]; if (infoLocal > 0) { //free(local_problem->q); //free(local_problem->M); //free(local_problem); /* Number of GS iterations */ options[0].iparam[1] = iter; fprintf(stderr, "lcp_NSGS_SBM error: Warning local LCP solver at global iteration %d.\n for block-row number %d. Output info equal to %d.\n", iter, rowNumber, infoLocal); //exit(EXIT_FAILURE); break; } while (localSolverNum < options->numberOfInternalSolvers - 1) localSolverNum++; } /* cblas_dcopy(problem->size , problem->q , 1 , w , 1); */ /* prod(problem->size,problem->size, 1.0, problem->M,z,1.0,w); */ /* cblas_daxpy(problem->size, -1.0, w,1,wBackup, 1); */ /* num = cblas_dnrm2(problem->size,wBackup,1); */ /* error = num*den; */ /* Criterium convergence */ hasNotConverged = lcp_compute_error(problem, z, w, tolerance, &error); /* if(error<tolerance) hasNotConverged = 0; */ } *info = hasNotConverged; /* Number of GS iterations */ options[0].iparam[1] = iter; /* Resulting error */ options[0].dparam[1] = error; free(local_problem->q); free(local_problem->M); free(local_problem); /* free(wBackup); */ }
void frictionContact3D_SOCLCP(FrictionContactProblem* problem, double *reaction, double *velocity, int* info, SolverOptions* options) { /* int and double parameters */ int* iparam = options->iparam; double* dparam = options->dparam; /* Number of contacts */ int nc = problem->numberOfContacts; /* Tolerance */ double tolerance = dparam[0]; if (options->numberOfInternalSolvers < 1) { numericsError("frictionContact3D_SOCLCP", "The SOCLCP for FrictionContactProblem needs options for the internal solvers, options[0].numberOfInternalSolvers should be >1"); } SolverOptions * internalsolver_options = options->internalSolvers; if (verbose > 0) { printf("Local solver data :"); printSolverOptions(internalsolver_options); } /***** Fixed Point Iterations *****/ double error = 1.; /* Current error */ soclcp_InternalSolverPtr internalsolver; SecondOrderConeLinearComplementarityProblem* soclcp = (SecondOrderConeLinearComplementarityProblem *)malloc(sizeof(SecondOrderConeLinearComplementarityProblem)); soclcp->n = problem->numberOfContacts * problem->dimension; soclcp->nc= problem->numberOfContacts; soclcp->M = problem-> M; soclcp->q = (double *) malloc(soclcp->n * sizeof(double)); soclcp->mu = problem->mu; soclcp->coneIndex = (unsigned int *) malloc((soclcp->nc+1) * sizeof(unsigned int)); for (int i=0; i < soclcp->n; i++) { soclcp->q[i]=problem->q[i]; } for (int i=0; i < soclcp->nc+1; i++) { soclcp->coneIndex[i] = 3*i; } if (internalsolver_options->solverId == SICONOS_SOCLCP_NSGS) { if (verbose == 1) printf(" ========================== Call NSGS solver SOCLCP problem ==========================\n"); internalsolver = &soclcp_nsgs; //internalsolver_options->internalSolvers->dWork = options->dWork; } else { fprintf(stderr, "Numerics, FrictionContact3D_SOCLCP failed. Unknown internal solver.\n"); exit(EXIT_FAILURE); } internalsolver_options->dparam[0]=options->dparam[0]; internalsolver_options->iparam[0]=options->iparam[0]; (*internalsolver)(soclcp, reaction , velocity , info , internalsolver_options); error = internalsolver_options->dparam[1]; double real_error=0.0; FrictionContact3D_compute_error(problem, reaction , velocity, tolerance, options, &real_error); if (options->callback) { options->callback->collectStatsIteration(options->callback->env, nc * 3, reaction, velocity, error, NULL); } if (verbose > 0) { printf("----------------------------------- FC3D - SOCLCP - # Iteration %i Final Error = %14.7e\n", internalsolver_options->iparam[7], error); printf("----------------------------------- FC3D - SOCLCP - # error of the real problem = %14.7e\n", real_error ); printf("----------------------------------- FC3D - SOCLCP - # gap with the real problem = %14.7e\n", fabs(real_error-error) ); } free(soclcp->q); free(soclcp->coneIndex); free(soclcp); if (internalsolver_options->internalSolvers != NULL) internalsolver_options->internalSolvers->dWork = NULL; dparam[0] = tolerance; dparam[1] = error; dparam[2] = fabs(real_error-error); iparam[7] = internalsolver_options->iparam[7]; }
void frictionContact3D_nsgs_velocity(FrictionContactProblem* problem, double *reaction, double *velocity, int* info, SolverOptions* options) { /* int and double parameters */ int* iparam = options->iparam; double* dparam = options->dparam; /* Number of contacts */ int nc = problem->numberOfContacts; NumericsMatrix* M = problem->M; /* Dimension of the problem */ int n = 3 * nc; /* Maximum number of iterations */ int itermax = iparam[0]; /* Tolerance */ double tolerance = dparam[0]; /* Check for trivial case */ /* *info = checkTrivialCase(n, q,velocity, reaction, options); */ if (*info == 0) return; SolverPtr local_solver = NULL; FreeSolverPtr freeSolver = NULL; ComputeErrorPtr computeError = NULL; if (M->storageType == 0) { /* /\* Inversion of the matrix M *\/ */ /* int* ipiv = (int *)malloc(n*sizeof(*ipiv)); */ /* int infoDGETRF=0; */ /* DGETRF(n,n,M->matrix0,n, ipiv,infoDGETRF ); */ /* assert(!infoDGETRF); */ /* int infoDGETRI; */ /* DGETRI(n,M->matrix0,n, ipiv,infoDGETRI ); */ /* assert(!infoDGETRI); */ /* double* qtmp = (double*)malloc(n*sizeof(double)); */ /* cblas_dcopy(n, q, 1, qtmp, 1); */ /* cblas_dgemv(CblasColMajor,CblasNoTrans, n, n, -1.0, M->matrix0 , n, qtmp,1,0.0,q, 1); */ /* free(ipiv); */ /* free(qtmp); */ double tolpinv = 1e-07; pinv(M->matrix0, n, n, tolpinv); } else { fprintf(stderr, "Numerics, frictionContact3D_nsgs_velocity. Not yet implemented for storageType %i\n", M->storageType); exit(EXIT_FAILURE); } if (options->numberOfInternalSolvers < 1) { numericsError("frictionContact3D_nsgs_velocity", "The NSGS method needs options for the internal solvers, options[0].numberOfInternalSolvers should be >1"); } SolverOptions * localsolver_options = options->internalSolvers; /* Connect local solver */ FrictionContactProblem* localproblem = 0; initializeLocalSolver_nsgs_velocity(&local_solver, &freeSolver, &computeError, problem, localproblem, localsolver_options); /***** NSGS_VELOCITY 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 */ dparam[0] = dparam[2]; // set the tolerance for the local solver while ((iter < itermax) && (hasNotConverged > 0)) { ++iter; /* Loop through the contact points */ //cblas_dcopy( n , q , incx , velocity , incy ); for (contact = 0 ; contact < nc ; ++contact) { (*local_solver)(localproblem, velocity, localsolver_options); for (int ncc = 0; ncc < 3; ncc ++) { printf("velocity[%i]=%14.7e\t", ncc, velocity[contact * 3 + ncc]); } printf("\n"); } /* **** Criterium convergence **** */ (*computeError)(problem, reaction , velocity, tolerance, options, &error); if (verbose > 0) printf("----------------------------------- FC3D - NSGS_VELOCITY - Iteration %i Error = %14.7e\n", iter, error); if (error < tolerance) hasNotConverged = 0; *info = hasNotConverged; } printf("----------------------------------- FC3D - NSGS_VELOCITY - # Iteration %i Final Error = %14.7e\n", iter, error); dparam[0] = tolerance; dparam[1] = error; iparam[7] = iter; /***** Free memory *****/ (*freeSolver)(); }