source: branches/2929_PrioritizedGrammarEnumeration/HeuristicLab.Algorithms.DataAnalysis.PGE/3.3/src/go-levmar/stack.c @ 16620

Last change on this file since 16620 was 16620, checked in by hmaislin, 9 months ago

#2929: Reorganized folder structure for make script, removed explicit marshalling, erased go-side logging

File size: 10.3 KB
Line 
1#include <stdlib.h>
2#include <stdio.h>
3#include <math.h>
4
5#include "levmar-2.6/levmar.h"
6#include "stack.h"
7
8
9#define STACK_BUF_LEN 128
10
11
12typedef struct {
13  double data[STACK_BUF_LEN];
14  int pos;
15} D_Stack;
16
17typedef struct {
18  int data[STACK_BUF_LEN];
19  int pos;
20} I_Stack;
21
22// typedef struct {
23//  double *data;
24//  int pos;
25// } D_StackD;
26
27// typedef struct {
28//  int *data;
29//  int pos;
30// } I_StackD;
31
32I_Stack* new_istack();
33int push_istack(I_Stack* stack, int i);
34void pop_istack(I_Stack* stack);
35int  top_istack(I_Stack* stack);
36int  len_istack(I_Stack* stack);
37int  get_istack(I_Stack* stack, int pos);
38int  is_empty_istack(I_Stack* stack);
39void clear_istack(I_Stack* stack);
40void free_istack(I_Stack* stack);
41
42D_Stack* new_dstack();
43int push_dstack(D_Stack* stack, double f);
44void pop_dstack(D_Stack* stack);
45double  top_dstack(D_Stack* stack);
46int  len_dstack(D_Stack* stack);
47double  get_dstack(D_Stack* stack, int pos);
48int  is_empty_dstack(D_Stack* stack);
49void clear_dstack(D_Stack* stack);
50void free_dstack(D_Stack* stack);
51
52I_Stack* new_istack() { 
53  I_Stack* stack = (I_Stack*) malloc(sizeof(I_Stack));
54  stack->pos = -1;
55  return stack;
56}
57int push_istack(I_Stack* stack, int i) { 
58  if (stack->pos +1 >= STACK_BUF_LEN)
59    return 0;
60  stack->pos++;
61  stack->data[stack->pos] = i;
62  return 1;
63}
64void pop_istack(I_Stack* stack) { 
65  if (stack->pos >= 0) stack->pos--;
66}
67int  top_istack(I_Stack* stack) { 
68  return stack->data[stack->pos];
69}
70int  len_istack(I_Stack* stack) { 
71  return stack->pos + 1;
72}
73int  get_istack(I_Stack* stack, int pos) { 
74  return stack->data[pos];
75}
76int  is_empty_istack(I_Stack* stack) { 
77  return (stack->pos < 0) ? 1 : 0;
78}
79void clear_istack(I_Stack* stack) { 
80  stack->pos = -1;
81}
82void free_istack(I_Stack* stack) { 
83  free(stack);
84}
85
86D_Stack* new_dstack() { 
87  D_Stack* stack = (D_Stack*) malloc(sizeof(D_Stack));
88  stack->pos = -1;
89  return stack;
90}
91int push_dstack(D_Stack* stack, double f) { 
92  if (stack->pos +1 >= STACK_BUF_LEN)
93    abort();
94    // return 0;
95  stack->pos++;
96  stack->data[stack->pos] = f;
97  return 1;
98}
99void pop_dstack(D_Stack* stack) { 
100  if (stack->pos >= 0) stack->pos--;
101}
102double  top_dstack(D_Stack* stack) { 
103  return stack->data[stack->pos];
104}
105int  len_dstack(D_Stack* stack) { 
106  return stack->pos + 1;
107}
108double  get_dstack(D_Stack* stack, int pos) { 
109  return stack->data[pos];
110}
111int  is_empty_dstack(D_Stack* stack) { 
112  return (stack->pos < 0) ? 1 : 0;
113}
114void clear_dstack(D_Stack* stack) { 
115  stack->pos = -1;
116}
117void free_dstack(D_Stack* stack) { 
118  free(stack);
119}
120
121
122
123
124
125
126
127
128double stack_eval(double t, double *c_in, double *x_in, D_Stack *d_stack, StackExpr expr );
129
130void stack_func( double *p, double *x, int m, int n, void *data) {
131  StackData *sdata = (StackData*)data;
132
133  int x_dim = sdata->x_dim;
134  double* x_data = sdata->x_data;
135  D_Stack d_stack;
136  // d_stack.clear();
137
138  // printf( "HELLO from stack_func\n");
139
140
141  int i;
142  for( i=0; i < n; i++ ) {
143    // printf( "%d %d %d\n",i,x_dim,sdata->x_len);
144    x[i] = stack_eval(0,p,&(x_data[i*x_dim]),&d_stack,sdata->expr);
145  }
146
147}
148
149void stack_jacfunc( double *p, double *jac, int m, int n, void *data) {
150  StackData *sdata = (StackData*)data;
151
152  int x_dim = sdata->x_dim;
153  double* x_data = sdata->x_data;
154  D_Stack d_stack;
155  // d_stack.clear();
156
157  int i,j;
158  for( i=0; i < n; i++ ) {
159    for( j=0; j < m; j++ ) {
160      jac[i*m+j] = stack_eval(0,p,&(x_data[i*x_dim]),&d_stack,sdata->derivs[j]);
161    }
162  }
163
164}
165
166
167void stack_levmar_der( double* ygiven, double* p, const int m, const int n, void* data ) {
168  double opts[LM_OPTS_SZ], info[LM_INFO_SZ];
169
170  // optimization control parameters; passing to levmar NULL instead of opts reverts to defaults
171  opts[0]=LM_INIT_MU; opts[1]=1E-15; opts[2]=1E-15; opts[3]=1E-20;
172  opts[4]=LM_DIFF_DELTA; // relevant only if the finite difference Jacobian version is used
173
174  // invoke the optimization function
175  dlevmar_der(stack_func, stack_jacfunc, p, ygiven, m, n, 1000, opts, info, NULL, NULL, data); // with analytic Jacobian
176}
177
178void stack_levmar_dif( double* ygiven, double* p, const int m, const int n, void* data ) {
179  double opts[LM_OPTS_SZ], info[LM_INFO_SZ];
180
181  // printf( "HELLO from stack_levmar_dif\n");
182  StackData *sdata = (StackData*)data;
183
184  int x_dim = sdata->x_dim;
185  int x_len = sdata->x_len;
186  // printf( "x_len: %d   x_dim: %d\n",x_len,x_dim);
187
188  // optimization control parameters; passing to levmar NULL instead of opts reverts to defaults
189  opts[0]=LM_INIT_MU; opts[1]=1E-15; opts[2]=1E-15; opts[3]=1E-20;
190  opts[4]=LM_DIFF_DELTA; // relevant only if the finite difference Jacobian version is used
191
192  // invoke the optimization function
193  dlevmar_dif(stack_func, p, ygiven, m, n, 1000, opts, info, NULL, NULL, data); // without Jacobian
194}
195
196
197/*
198ExprTypes:
199---------------
200NULL:      0
201STARTLEAF: 1
202CONSTANT:  2
203TIME:      4
204SYSTEM:    5
205VAR:       6
206LASTLEAF:  7
207STARTFUNC: 8
208NEG:       9
209ABS:       10
210SQRT:      11
211SIN:       12
212COS:       13
213TAN:       14
214EXP:       15
215LASTFUNC:  17
216POWI:      18
217POWF:      19
218POWE:      20
219DIV:       21
220ADD:       22
221MUL:       23
222EXPR_MAX:  24
223STARTVAR:  25
224*/
225
226
227// #define print_stack_eval 1
228
229double stack_eval(double t, double *c_in, double *x_in, D_Stack *d_stack, StackExpr expr ) {
230
231  // I_Stack *serial = new_istack();
232  // I_Stack istack;
233  // I_Stack *serial = &istack;
234
235  clear_dstack(d_stack);
236  int* serial = expr.serial;
237
238 
239  #ifdef print_stack_eval
240  printf("Serial: |%d|", expr.s_len);
241  #endif
242  int s;
243  for( s=0; s < expr.s_len; s++ ) {
244  // #ifdef print_stack_eval
245  //  printf( "%d ", expr.serial[s]);
246  // #endif
247  //  push_istack(serial,expr.serial[expr.s_len-s-1]);
248  // }
249  // #ifdef print_stack_eval
250  // printf("\n");
251  // #endif
252 
253  // clear_dstack(d_stack);
254
255  // // fill i_stack with cmds and d_stack with leaves
256  // while ( !is_empty_istack(serial) ) {
257  //  // printf( "processing serial\n");
258    // int val = top_istack(serial);
259    // pop_istack(serial);
260
261    int val = serial[s];
262
263  #ifdef print_stack_eval
264    int dlen = len_dstack(d_stack);
265    int slen = len_istack(serial);
266    printf( "S: %d    val:  %d   \n", slen, val );
267    printf( "serial(%d): [ ", slen); 
268    for( i=0; i < slen; i++ )
269      printf( "%d ", get_istack(serial,i) );
270    printf(" ]\n");
271    printf( "d_stack(%d): [ ", dlen); 
272    for( i=0; i < dlen; i++ )
273      printf( "%.2f ", get_dstack(d_stack,i) );
274    printf(" ]\n");
275  #endif
276
277    switch (val) {
278     
279      // CONSTANT:  2
280      case 2: {
281        s++;
282        int p = serial[s];
283        // int p = top_istack(serial);
284        // pop_istack(serial);
285        push_dstack(d_stack,c_in[p]);
286      }
287        break; 
288
289      // HACK***
290      // CONSTANTF:  3
291      case 3: {
292        s++;
293        int p = serial[s];
294        // int p = top_istack(serial);
295        // pop_istack(serial);
296        push_dstack(d_stack,p);
297      }
298        break; 
299      // TIME:      4
300      case 4:
301        push_dstack(d_stack,t);
302        break;
303      // SYSTEM:    5
304      // case 5:
305        // s++;
306        // push_dstack(d_stack,sys_in[serial[s]]);
307      // VAR:       6   should already be transformed, but just in case
308      case 6: {
309        s++;
310        int p = serial[s];
311        // int p = top_istack(serial);
312        // pop_istack(serial);
313        push_dstack(d_stack,x_in[p]);
314        break;
315      }
316      // NEG:       9
317      case 9: {
318        double top = top_dstack(d_stack);
319        pop_dstack(d_stack);
320        push_dstack(d_stack, -top);
321      }
322        break;
323      // ABS:       10
324      case 10: {
325        double top = top_dstack(d_stack);
326        pop_dstack(d_stack);
327        push_dstack(d_stack, fabs(top));
328      }
329        break;
330      // SQRT:      11
331      case 11: {
332        double top = top_dstack(d_stack);
333        pop_dstack(d_stack);
334        push_dstack(d_stack, sqrt(top));
335      }
336        break;
337      // SIN:       12
338      case 12: {
339        double top = top_dstack(d_stack);
340        pop_dstack(d_stack);
341        push_dstack(d_stack, sin(top));
342      }
343        break;
344      // COS:       13
345      case 13: {
346        double top = top_dstack(d_stack);
347        pop_dstack(d_stack);
348        push_dstack(d_stack, cos(top));
349      }
350        break;
351      // TAN:       14
352      case 14: {
353        double top = top_dstack(d_stack);
354        pop_dstack(d_stack);
355        push_dstack(d_stack, tan(top));
356      }
357        break;
358      // EXP:       15
359      case 15: {
360        double top = top_dstack(d_stack);
361        pop_dstack(d_stack);
362        push_dstack(d_stack, exp(top));
363      }
364        break;
365      // LOG:       16
366      case 16: {
367        double top = top_dstack(d_stack);
368        pop_dstack(d_stack);
369        push_dstack(d_stack, log(top));
370      }
371        break;
372
373            // POWI:      18
374      case 18: {
375        double top = top_dstack(d_stack);
376        pop_dstack(d_stack);
377        s++;
378        int pwr = serial[s];
379        // int pwr = top_istack(serial);
380        // pop_istack(serial);
381        push_dstack(d_stack, pow(top,pwr));
382      }
383        break;
384      // POWE:      20
385      case 20: {
386          double top = top_dstack(d_stack);
387          pop_dstack(d_stack);
388          double pwr = top_dstack(d_stack);
389          pop_dstack(d_stack);
390          push_dstack(d_stack, pow(top,pwr));
391        }
392        break;
393      // DIV:       21
394      case 21: {
395          double denom = top_dstack(d_stack);
396          pop_dstack(d_stack);
397          double numer = top_dstack(d_stack);
398          pop_dstack(d_stack);
399          push_dstack(d_stack, numer / denom);
400        }
401        break;
402      // ADD:       22
403      case 22: {
404          double lhs = top_dstack(d_stack);
405          pop_dstack(d_stack);
406          double rhs = top_dstack(d_stack);
407          pop_dstack(d_stack);
408          push_dstack(d_stack, lhs + rhs);
409          // printf( "%f + %f = %f\n", lhs, rhs, top_dstack(d_stack));
410        }
411        break;
412      // MUL:       23
413      case 23: {
414          double lhs = top_dstack(d_stack);
415          pop_dstack(d_stack);
416          double rhs = top_dstack(d_stack);
417          pop_dstack(d_stack);
418          push_dstack(d_stack, lhs * rhs);
419          // printf( "%f * %f = %f\n", lhs, rhs, top_dstack(d_stack));
420        }
421        break;
422
423
424
425
426      case 0:
427      default:
428        // STARTVAR:  25
429        if (val >= 25) {
430          push_dstack(d_stack,x_in[val-25]); // x_dim_of_var = val - STARTVAR
431        } else {
432          printf("pushing unknown cmd %d\n", val);
433          int s;
434          for( s=0; s < expr.s_len; s++ ) {
435            printf( "%d ", expr.serial[s]);
436          }
437          printf( "\n");
438          abort();
439        }
440    }
441
442  #ifdef print_stack_eval
443    dlen = len_dstack(d_stack);
444    printf( "S: %d val: %d\n", s, val);
445    printf( "d_stack(%d): [ ", dlen); 
446    for( i=0; i < dlen; i++ )
447      printf( "%.2f ", get_dstack(d_stack,i) );
448    printf(" ]\n\n");
449  #endif
450  }
451
452  // free(serial);
453  return top_dstack(d_stack);
454
455}
456
457
Note: See TracBrowser for help on using the repository browser.