1 /* GAMS stuff */
2 
3 #define _XOPEN_SOURCE 700
4 
5 #if defined (__unix__) || (defined (__APPLE__) && defined (__MACH__))
6 #include <unistd.h>
7 #endif
8 
9 #include <stddef.h>
10 #include <stdlib.h>
11 #include <string.h>
12 
13 #if defined(_POSIX_VERSION) && (_POSIX_VERSION >= 200112L)
14 #elif _MSC_VER
15 #define strdup _strdup
16 #else
strdup(const char * src)17 static inline char* strdup(const char* src)
18 {
19   size_t len = strlen(src) + 1;
20   char* dest = (char*)malloc(len * sizeof(char));
21   strncpy(dest, src, len);
22   return dest;
23 }
24 #endif
25 
26 #include "SiconosConfig.h" // for HAVE_GAMS_C_API
27 #include <math.h>
28 
29 #ifdef HAVE_GAMS_C_API
30 #include <stdio.h>
31 #include <assert.h>
32 #include <stdbool.h>
33 #include <float.h>
34 
35 #include "CSparseMatrix_internal.h"
36 #include "NumericsMatrix.h"
37 #include "FrictionContactProblem.h"
38 #include "SolverOptions.h"
39 
40 #include "GAMSlink.h"
41 
42 
43 
44 #include "sanitizer.h"
45 
46 #define DEBUG_NOCOLOR
47 //#define DEBUG_STDOUT
48 //#define DEBUG_MESSAGES
49 #include "siconos_debug.h"
50 
51 #define ETERMINATE 4242
52 
53 #define TOTAL_TIME_USED 2
54 
55 #define TOTAL_ITER 2
56 #define LAST_MODEL_STATUS 3
57 #define LAST_SOLVE_STATUS 4
58 
59 //#define SMALL_APPROX
60 
61 #include <fcntl.h>
62 #include <unistd.h>
63 #include <errno.h>
64 
cp(const char * to,const char * from)65 static int cp(const char *to, const char *from)
66 {
67   int fd_to, fd_from;
68   char buf[4096];
69   ssize_t nread;
70   int saved_errno;
71 
72   fd_from = open(from, O_RDONLY);
73   if(fd_from < 0)
74     return -1;
75 
76   fd_to = open(to, O_WRONLY | O_CREAT | O_EXCL, 0666);
77   if(fd_to < 0)
78     goto out_error;
79 
80   while(nread = read(fd_from, buf, sizeof buf), nread > 0)
81   {
82     char *out_ptr = buf;
83     ssize_t nwritten;
84 
85     do
86     {
87       nwritten = write(fd_to, out_ptr, nread);
88 
89       if(nwritten >= 0)
90       {
91         nread -= nwritten;
92         out_ptr += nwritten;
93       }
94       else if(errno != EINTR)
95       {
96         goto out_error;
97       }
98     }
99     while(nread > 0);
100   }
101 
102   if(nread == 0)
103   {
104     if(close(fd_to) < 0)
105     {
106       fd_to = -1;
107       goto out_error;
108     }
109     close(fd_from);
110 
111     /* Success! */
112     return 0;
113   }
114 
115 out_error:
116   saved_errno = errno;
117 
118   close(fd_from);
119   if(fd_to >= 0)
120     close(fd_to);
121 
122   errno = saved_errno;
123   return -1;
124 }
125 
setDashedOptions(const char * optName,const char * optValue,const char * paramFileName)126 static void setDashedOptions(const char* optName, const char* optValue, const char* paramFileName)
127 {
128   FILE* f = fopen(paramFileName, "a");
129   if(f)
130   {
131     fprintf(f, "%s %s\n", optName, paramFileName);
132     fclose(f);
133   }
134   else
135   {
136     printf("Failed to create option %s with value %s in %s\n", optName, optValue, paramFileName);
137   }
138 }
139 
filename_datafiles(const int iter,const int solverId,const char * base_name,unsigned len,char * template_name,char * log_filename)140 void filename_datafiles(const int iter, const int solverId, const char* base_name, unsigned len, char* template_name, char* log_filename)
141 {
142   char iterStr[40];
143   snprintf(iterStr, sizeof(iterStr), "-i%d-%s", iter, solver_options_id_to_name(solverId));
144   if(base_name)
145   {
146     strncpy(template_name, base_name, len);
147     strncpy(log_filename, base_name, len);
148   }
149   else
150   {
151     strncpy(template_name, "fc3d_avi-condensed", len);
152     strncpy(log_filename, "fc3d_avi-condense-log", len);
153   }
154 
155   strncat(template_name, iterStr, len - strlen(template_name) - 1);
156   strncat(log_filename, iterStr, len - strlen(log_filename) - 1);
157   strncat(log_filename, ".log", len - strlen(log_filename) - 1);
158 }
159 
160 
161 
NM_to_GDX(idxHandle_t Xptr,const char * name,const char * descr,NumericsMatrix * M)162 int NM_to_GDX(idxHandle_t Xptr, const char* name, const char* descr, NumericsMatrix* M)
163 {
164   char msg[GMS_SSSIZE];
165 
166   int dims[2];
167   dims[0] = M->size0;
168   dims[1] = M->size1;
169   if(idxDataWriteStart(Xptr, name, descr, 2, dims, msg, GMS_SSSIZE) == 0)
170     idxerrorR(idxGetLastError(Xptr), "idxDataWriteStart");
171 
172   switch(M->storageType)
173   {
174   case NM_DENSE:
175   {
176     assert(M->matrix0);
177     idxDataWriteDenseColMajor(Xptr, 2, M->matrix0);
178     break;
179   }
180   case NM_SPARSE_BLOCK: /* Perform a conversion to sparse storage */
181   case NM_SPARSE:
182   {
183     CSparseMatrix* cs = NM_csc(M);
184     assert(cs->p);
185     assert(cs->i);
186     assert(cs->x);
187     int* p_int = (int*)malloc((cs->n+1) * sizeof(int));
188     int* i_int = (int*)malloc(cs->nzmax * sizeof(int));
189     assert(cs->n == M->size1);
190     assert(cs->m == M->size0);
191     for(unsigned i = 0; i < cs->n+1; ++i)
192     {
193       p_int[i] = (int) cs->p[i];
194     }
195 
196     for(unsigned i = 0; i < cs->nzmax; ++i)
197     {
198       i_int[i] = (int) cs->i[i];
199     }
200 
201     idxDataWriteSparseColMajor(Xptr, p_int, i_int, cs->x);
202 
203     free(p_int);
204     free(i_int);
205     break;
206   }
207   default:
208   {
209     printf("NM_to_GDX :: unsupported matrix storage\n");
210     exit(EXIT_FAILURE);
211   }
212   }
213 
214 
215   if(0==idxDataWriteDone(Xptr))
216     idxerrorR(idxGetLastError(Xptr), "idxDataWriteDone");
217 
218   return 0;
219 }
SN_gams_solve(unsigned iter,optHandle_t Optr,char * sysdir,char * model,const char * base_name,SolverOptions * options,SN_GAMS_gdx * gdx_data)220 int SN_gams_solve(unsigned iter, optHandle_t Optr, char* sysdir, char* model, const char* base_name, SolverOptions* options, SN_GAMS_gdx* gdx_data)
221 {
222   assert(gdx_data);
223   SN_GAMS_NM_gdx* mat_for_gdx = gdx_data->mat_for_gdx;
224   SN_GAMS_NV_gdx* vec_for_gdx = gdx_data->vec_for_gdx;
225   SN_GAMS_NV_gdx* vec_from_gdx = gdx_data->vec_from_gdx;
226 
227   char msg[GMS_SSSIZE];
228   int status;
229   gamsxHandle_t Gptr = NULL;
230   idxHandle_t Xptr = NULL;
231   gmoHandle_t gmoPtr = NULL;
232   double infos[4] = {0.};
233   /* Create objects */
234 
235   DEBUG_PRINT("FC3D_AVI_GAMS :: creating gamsx object\n");
236   if(! gamsxCreateD(&Gptr, sysdir, msg, sizeof(msg)))
237   {
238     fprintf(stderr, "Could not create gamsx object: %s\n", msg);
239     return 1;
240   }
241 
242   DEBUG_PRINT("FC3D_AVI_GAMS :: creating gdx object\n");
243   if(! idxCreateD(&Xptr, sysdir, msg, sizeof(msg)))
244   {
245     fprintf(stderr, "Could not create gdx object: %s\n", msg);
246     return 1;
247   }
248 
249   DEBUG_PRINT("FC3D_AVI_GAMS :: creating gmo object\n");
250   if(! gmoCreateD(&gmoPtr, sysdir, msg, sizeof(msg)))
251   {
252     fprintf(stderr, "Could not create gmo object: %s\n", msg);
253     return 1;
254   }
255 
256   /* create input and output gdx names*/
257   char gdxFileName[GMS_SSSIZE];
258   char solFileName[GMS_SSSIZE];
259 //  char paramFileName[GMS_SSSIZE];
260 
261   strncpy(gdxFileName, base_name, sizeof(gdxFileName));
262   strncpy(solFileName, base_name, sizeof(solFileName));
263   strncat(solFileName, "_sol", sizeof(solFileName) - strlen(solFileName) - 1);
264 
265   strncat(gdxFileName, ".gdx", sizeof(gdxFileName) - strlen(gdxFileName) - 1);
266   strncat(solFileName, ".gdx", sizeof(solFileName) - strlen(solFileName) - 1);
267 //  strncat(paramFileName, ".txt", sizeof(paramFileName));
268 
269   /* XXX ParmFile is not a string option */
270 //  optSetStrStr(Optr, "ParmFile", paramFileName);
271 //  setDashedOptions("filename", gdxFileName, paramFileName);
272   optSetStrStr(Optr, "User1", gdxFileName);
273   optSetStrStr(Optr, "User2", solFileName);
274 
275   idxOpenWrite(Xptr, gdxFileName, "Siconos/Numerics NM_to_GDX", &status);
276   if(status)
277     idxerrorR(status, "idxOpenWrite");
278   DEBUG_PRINT("FC3D_AVI_GAMS :: fc3d_avi-condensed.gdx opened\n");
279 
280   while(mat_for_gdx)
281   {
282     char mat_descr[30];
283     assert(mat_for_gdx->name);
284     assert(mat_for_gdx->mat);
285     snprintf(mat_descr, sizeof(mat_descr), "%s matrix", mat_for_gdx->name);
286     if((status=NM_to_GDX(Xptr, mat_for_gdx->name, mat_descr, mat_for_gdx->mat)))
287     {
288       fprintf(stderr, "Model data for matrix %s not written\n", mat_for_gdx->name);
289       infos[1] = (double)-ETERMINATE;
290       goto fail;
291     }
292     DEBUG_PRINTF("GAMSlink :: %s matrix written\n", mat_for_gdx->name);
293     mat_for_gdx = mat_for_gdx->next;
294   }
295 
296   while(vec_for_gdx)
297   {
298     char vec_descr[30];
299     assert(vec_for_gdx->name);
300     assert(vec_for_gdx->vec);
301     assert(vec_for_gdx->size > 0);
302     snprintf(vec_descr, sizeof(vec_descr), "%s vector", vec_for_gdx->name);
303 
304     if((status=NV_to_GDX(Xptr, vec_for_gdx->name, vec_descr, vec_for_gdx->vec, vec_for_gdx->size)))
305     {
306       fprintf(stderr, "Model data for vector %s not written\n", vec_for_gdx->name);
307       infos[1] = (double)-ETERMINATE;
308       goto fail;
309     }
310     DEBUG_PRINTF("FC3D_AVI_GAMS :: %s vector written\n", vec_for_gdx->name);
311     vec_for_gdx = vec_for_gdx->next;
312 
313   }
314 
315   if(idxClose(Xptr))
316     idxerrorR(idxGetLastError(Xptr), "idxClose");
317 
318 
319 //   cp(gdxFileName, "fc3d_avi-condensed.gdx");
320 
321 
322   if((status=CallGams(Gptr, Optr, sysdir, model)))
323   {
324     fprintf(stderr, "Call to GAMS failed\n");
325     infos[1] = (double)-ETERMINATE;
326     goto fail;
327   }
328 
329 
330   /************************************************
331    * Read back solution
332    ************************************************/
333   idxOpenRead(Xptr, solFileName, &status);
334   if(status)
335     idxerrorR(status, "idxOpenRead");
336 
337   while(vec_from_gdx)
338   {
339     assert(vec_from_gdx->name);
340     assert(vec_from_gdx->vec);
341     assert(vec_from_gdx->size > 0);
342     double* data = vec_from_gdx->vec;
343     unsigned size = vec_from_gdx->size;
344     /* GAMS does not set a value to 0 ... --xhub */
345     memset(data, 0, size*sizeof(double));
346     if((status=GDX_to_NV(Xptr, vec_from_gdx->name, data, size)))
347     {
348       fprintf(stderr, "Model data %s could not be read\n", vec_from_gdx->name);
349       infos[1] = (double)-ETERMINATE;
350       goto fail;
351     }
352     vec_from_gdx = vec_from_gdx->next;
353   }
354 
355   if((status=GDX_to_NV(Xptr, "infos", infos, sizeof(infos)/sizeof(double))))
356   {
357     fprintf(stderr, "infos could not be read\n");
358     infos[1] = (double)-ETERMINATE;
359     goto fail;
360   }
361 
362   if(idxClose(Xptr))
363     idxerrorR(idxGetLastError(Xptr), "idxClose");
364 
365   options->iparam[TOTAL_ITER] += (int)infos[2];
366   options->iparam[LAST_MODEL_STATUS] = (int)infos[0];
367   options->iparam[LAST_SOLVE_STATUS] = (int)infos[1];
368   options->dparam[TOTAL_TIME_USED] += infos[3];
369   printf("SolveStat = %d, ModelStat = %d\n", (int)infos[1], (int)infos[0]);
370   gmoGetModelStatusTxt(gmoPtr, (int)infos[0], msg);
371   DEBUG_PRINTF("%s\n", msg);
372   gmoGetSolveStatusTxt(gmoPtr, (int)infos[1], msg);
373   DEBUG_PRINTF("%s\n", msg);
374 
375 fail:
376   idxFree(&Xptr);
377   gamsxFree(&Gptr);
378   gmoFree(&gmoPtr);
379   return (int)infos[1];
380 }
381 
382 #define GAMS_ADD_OPT(GAMSP_OPT_L, GAMSP_OPT_T) \
383 GAMSP_OPT_T* next_opt = GAMSP_OPT_L; \
384 GAMSP_OPT_T* new_opt; \
385 if (next_opt) \
386 { \
387   while (next_opt->next_opt) \
388   { \
389     next_opt = next_opt->next_opt; \
390   } \
391   next_opt->next_opt = (GAMSP_OPT_T*)malloc(sizeof(GAMSP_OPT_T)); \
392   new_opt = next_opt->next_opt; \
393 } \
394 else \
395 { \
396   GAMSP_OPT_L = (GAMSP_OPT_T*)malloc(sizeof(GAMSP_OPT_T)); \
397   new_opt = GAMSP_OPT_L; \
398 } \
399 new_opt->name = lname; \
400 new_opt->value = value; \
401 new_opt->type = type; \
402 new_opt->next_opt = NULL; \
403 
404 #define GAMS_ADD_PREP(GP, name) \
405 assert(GP); \
406 assert(name); \
407 char* lname = strdup(name);
408 
add_GAMS_opt_str(SN_GAMSparams * GP,char * name,char * value_orig,unsigned type)409 void add_GAMS_opt_str(SN_GAMSparams* GP, char* name, char* value_orig, unsigned type)
410 {
411   GAMS_ADD_PREP(GP, name);
412   assert(value_orig);
413   char* value = strdup(value_orig);
414   GAMS_ADD_OPT(GP->opt_str_list, GAMS_opt_str);
415 }
416 
add_GAMS_opt_bool(SN_GAMSparams * GP,char * name,bool value,unsigned type)417 void add_GAMS_opt_bool(SN_GAMSparams* GP, char* name, bool value, unsigned type)
418 {
419   GAMS_ADD_PREP(GP, name);
420   GAMS_ADD_OPT(GP->opt_bool_list, GAMS_opt_bool);
421 }
422 
add_GAMS_opt_int(SN_GAMSparams * GP,char * name,int value,unsigned type)423 void add_GAMS_opt_int(SN_GAMSparams* GP, char* name, int value, unsigned type)
424 {
425   GAMS_ADD_PREP(GP, name);
426   GAMS_ADD_OPT(GP->opt_int_list, GAMS_opt_int);
427 }
428 
add_GAMS_opt_double(SN_GAMSparams * GP,char * name,double value,unsigned type)429 void add_GAMS_opt_double(SN_GAMSparams* GP, char* name, double value, unsigned type)
430 {
431   GAMS_ADD_PREP(GP, name);
432   GAMS_ADD_OPT(GP->opt_double_list, GAMS_opt_double);
433 }
434 
createGAMSparams(char * model_dir,char * gams_dir)435 SN_GAMSparams* createGAMSparams(char* model_dir, char* gams_dir)
436 {
437   SN_GAMSparams* GP = (SN_GAMSparams*) malloc(sizeof(SN_GAMSparams));
438 
439   GP->model_dir = strdup(model_dir);
440   GP->gams_dir = strdup(gams_dir);
441   assert(GP->model_dir);
442   assert(GP->gams_dir);
443   GP->filename = NULL;
444   GP->filename_suffix = NULL;
445   GP->opt_str_list = NULL;
446   GP->opt_bool_list = NULL;
447   GP->opt_int_list = NULL;
448   GP->opt_double_list = NULL;
449 
450   return GP;
451 }
452 
deleteGAMSparams(SN_GAMSparams * GP)453 void deleteGAMSparams(SN_GAMSparams* GP)
454 {
455   if(GP->model_dir)
456   {
457     free(GP->model_dir);
458     GP->model_dir = NULL;
459   }
460 
461   if(GP->gams_dir)
462   {
463     free(GP->gams_dir);
464     GP->gams_dir = NULL;
465   }
466 
467   if(GP->opt_str_list)
468   {
469     GAMS_opt_str* next_opt = GP->opt_str_list;
470     do
471     {
472       GAMS_opt_str* str_opt = next_opt;
473       next_opt = str_opt->next_opt;
474       assert(str_opt->name);
475       free(str_opt->name);
476       str_opt->name = NULL;
477       assert(str_opt->value);
478       free(str_opt->value);
479       str_opt->value = NULL;
480       str_opt->next_opt = NULL;
481       free(str_opt);
482     }
483     while(next_opt);
484     GP->opt_str_list = NULL;
485   }
486   if(GP->opt_bool_list)
487   {
488     GAMS_opt_bool* next_opt = GP->opt_bool_list;
489     do
490     {
491       GAMS_opt_bool* bool_opt = next_opt;
492       next_opt = bool_opt->next_opt;
493       bool_opt->name = NULL;
494       bool_opt->value = false;
495       bool_opt->next_opt = NULL;
496       free(bool_opt);
497     }
498     while(next_opt);
499     GP->opt_bool_list = NULL;
500   }
501   if(GP->opt_int_list)
502   {
503     GAMS_opt_int* next_opt = GP->opt_int_list;
504     do
505     {
506       GAMS_opt_int* int_opt = next_opt;
507       next_opt = int_opt->next_opt;
508       int_opt->name = NULL;
509       int_opt->value = 0;
510       int_opt->next_opt = NULL;
511       free(int_opt);
512     }
513     while(next_opt);
514     GP->opt_int_list = NULL;
515   }
516   if(GP->opt_double_list)
517   {
518     GAMS_opt_double* next_opt = GP->opt_double_list;
519     do
520     {
521       GAMS_opt_double* double_opt = next_opt;
522       next_opt = double_opt->next_opt;
523       double_opt->name = NULL;
524       double_opt->value = 0.;
525       double_opt->next_opt = NULL;
526       free(double_opt);
527     }
528     while(next_opt);
529     GP->opt_double_list = NULL;
530   }
531   free(GP);
532 }
533 
534 /*
535 static void FC3D_gams_generate_first_constraints(NumericsMatrix* Akmat, double* mus)
536 {
537   unsigned nb_contacts = (unsigned)Akmat->size1/3;
538   assert(nb_contacts*3 == (unsigned)Akmat->size1);
539   unsigned nb_approx = (unsigned)Akmat->size0/nb_contacts;
540   assert(nb_approx*nb_contacts == (unsigned)Akmat->size0);
541   unsigned offset_row = 0;
542   CSparseMatrix* triplet_mat = Akmat->matrix2->triplet;
543 
544   double angle = 2*M_PI/(NB_APPROX + 1);
545   DEBUG_PRINTF("angle: %g\n", angle);
546 
547   for (unsigned j = 0; j < nb_contacts; ++j)
548   {
549     double mu = mus[j];
550     for (unsigned i = 0; i < nb_approx; ++i)
551     {
552       cs_entry(triplet_mat, i + offset_row, 3*j, mu);
553       cs_entry(triplet_mat, i + offset_row, 3*j + 1, cos(i*angle));
554       cs_entry(triplet_mat, i + offset_row, 3*j + 2, sin(i*angle));
555     }
556     offset_row += nb_approx;
557   }
558 }
559 */
560 #endif
561 
562