c_function.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ACADO Toolkit.
3  *
4  * ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
5  * Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
6  * Milan Vukov, Rien Quirynen, KU Leuven.
7  * Developed within the Optimization in Engineering Center (OPTEC)
8  * under supervision of Moritz Diehl. All rights reserved.
9  *
10  * ACADO Toolkit is free software; you can redistribute it and/or
11  * modify it under the terms of the GNU Lesser General Public
12  * License as published by the Free Software Foundation; either
13  * version 3 of the License, or (at your option) any later version.
14  *
15  * ACADO Toolkit is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18  * Lesser General Public License for more details.
19  *
20  * You should have received a copy of the GNU Lesser General Public
21  * License along with ACADO Toolkit; if not, write to the Free Software
22  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
23  *
24  */
25 
26 
27 
38 
39 
41 
42 
43 
44 //
45 // PUBLIC MEMBER FUNCTIONS:
46 //
47 
49 
50  cFcn = NULL;
51  cFcnDForward = NULL;
52  cFcnDBackward = NULL;
53  dim = 0 ;
54 
55  user_data = 0;
56 
57  initialize();
58 }
59 
60 
62 
63  cFcn = cFcn_;
64  cFcnDForward = NULL ;
65  cFcnDBackward = NULL ;
66  dim = dim_ ;
67 
68  user_data = 0;
69 
70  initialize();
71 }
72 
73 
75  cFcnPtr cFcn_ ,
76  cFcnDPtr cFcnDForward_ ,
77  cFcnDPtr cFcnDBackward_ ){
78 
79  cFcn = cFcn_ ;
80  cFcnDForward = cFcnDForward_ ;
81  cFcnDBackward = cFcnDBackward_;
82  dim = dim_ ;
83 
84  user_data = 0;
85 
86  initialize();
87 }
88 
89 
90 CFunction:: CFunction( const CFunction& arg ){ copy (arg); }
92 
94 
95  if ( this != &arg ){
96  deleteAll();
97  copy(arg);
98  }
99  return *this;
100 }
101 
102 
104 
105  nn = 0;
106  maxAlloc = 1;
107 
108  xStore = (double**)calloc(maxAlloc, sizeof(double*));
109  seedStore = (double**)calloc(maxAlloc, sizeof(double*));
110 
111  xStore [0] = 0;
112  seedStore[0] = 0;
113 
114  return SUCCESSFUL_RETURN;
115 }
116 
117 
118 void CFunction::copy( const CFunction &arg ){
119 
120  uint run1, run2;
121 
122  if( arg.cFcn == 0 ) cFcn = 0 ;
123  else cFcn = arg.cFcn ;
124  if( arg.cFcnDForward == 0 ) cFcnDForward = 0 ;
125  else cFcnDForward = arg.cFcnDForward ;
126  if( arg.cFcnDBackward == 0 ) cFcnDBackward = 0 ;
127  else cFcnDBackward = arg.cFcnDBackward;
128 
129  dim = arg.dim;
130  nn = arg.nn ;
131 
132  user_data = arg.user_data;
133 
134  maxAlloc = arg.maxAlloc;
135 
136  xStore = (double**)calloc(maxAlloc,sizeof(double*));
137  seedStore = (double**)calloc(maxAlloc,sizeof(double*));
138 
139  for( run1 = 0; run1 < maxAlloc; run1++ ){
140  if( nn != 0 ){
141  xStore[run1] = new double[nn];
142  seedStore[run1] = new double[nn];
143  for( run2 = 0; run2 < nn; run2++ ){
144  xStore[run1][run2] = arg.xStore [run1][run2];
145  seedStore[run1][run2] = arg.seedStore[run1][run2];
146  }
147  }
148  else{
149  xStore[run1] = 0;
150  seedStore[run1] = 0;
151  }
152  }
153 }
154 
155 
157 
158  uint run1;
159 
160  for( run1 = 0; run1 < maxAlloc; run1++ ){
161  if( xStore != 0 ) delete[] xStore [run1];
162  if( seedStore != 0 ) delete[] seedStore[run1];
163  }
164  if( xStore != 0 ) free(xStore );
165  if( seedStore != 0 ) free(seedStore);
166 }
167 
168 
170 
171  uint run1,run2;
172 
173  CFunction thisFunction(*this);
174  thisFunction.nn = arg.getDim();
175 
176  for( run1 = 0; run1 < maxAlloc; run1++ ){
177  thisFunction.xStore [run1] = new double[thisFunction.nn];
178  thisFunction.seedStore[run1] = new double[thisFunction.nn];
179  for( run2 = 0; run2 < thisFunction.nn; run2++ ){
180  thisFunction.xStore [run1][run2] = 0.0;
181  thisFunction.seedStore[run1][run2] = 0.0;
182  }
183  }
184 
185  Expression tmp(dim);
186 
187  COperator dummy;
188  dummy.increaseID();
189 
190  for( run1 = 0; run1 < dim; run1++ ){
191  delete tmp.element[run1];
192  tmp.element[run1] = new COperator( thisFunction, arg, run1 );
193  }
194 
195  return tmp;
196 }
197 
198 
200 
201  return dim;
202 }
203 
204 
205 returnValue CFunction::evaluate( double *x, double *result ){
206 
207  if( cFcn != 0 ) cFcn( x, result, user_data );
208  else evaluateCFunction( x, result );
209 
210  return SUCCESSFUL_RETURN;
211 }
212 
213 
214 
215 void CFunction::evaluateCFunction( double *x, double *result ){
216 
217  // DUMMY IMPLEMENTATION, CAN BE IMPLEMENTED BY THE USER
218  // IN DERIVED CLASSES.
219 }
220 
221 
222 
223 returnValue CFunction::evaluate( double *x, double *result, PrintLevel printL ){
224 
225  uint run1;
226 
227  evaluate(x,result);
228 
229  if (printL == MEDIUM || printL == HIGH)
230  {
231  std::cout << "C-function evaluation: ";
232  for (run1 = 0; run1 < dim; run1++)
233  std::cout << "f[" << run1 << "] = " << std::scientific << result[ run1 ] << std::endl;
234  }
235  return SUCCESSFUL_RETURN;
236 }
237 
238 
239 returnValue CFunction::evaluate( int number, double *x, double *result ){
240 
241  uint run1;
242  uint run2;
243 
244  evaluate(x,result);
245 
246  if( number >= (int) maxAlloc ){
247 
248  xStore= (double**)realloc(xStore, (number+1)*sizeof(double*));
249  for( run1 = maxAlloc; (int) run1 < number+1; run1++ ){
250  xStore[run1] = new double[nn];
251  }
252  seedStore = (double**)realloc(seedStore, (number+1)*sizeof(double*));
253  for( run1 = maxAlloc; (int) run1 < number+1; run1++ ){
254  seedStore[run1] = new double[nn];
255  }
256  maxAlloc = number+1;
257  }
258 
259  for( run2 = 0; run2 < nn; run2++ ){
260  xStore[number][run2] = x[run2];
261  }
262 
263  return SUCCESSFUL_RETURN;
264 }
265 
266 
267 returnValue CFunction::AD_forward( double *x, double *seed, double *f, double *df ){
268 
269  if( cFcnDForward != NULL ){
270  cFcnDForward( 0, x, seed, f, df, user_data );
271  return SUCCESSFUL_RETURN;
272  }
273  else{
274 
275  uint run1;
276 
277  if( cFcn != 0 )
278  cFcn( x, f, user_data );
279  else evaluateCFunction( x, f );
280 
281  for( run1 = 0; run1 < nn; run1++ ){
282  x[run1] = x[run1] + SQRT_EPS*seed[run1];
283  }
284 
285  if( cFcn != 0 )
286  cFcn( x, df, user_data );
287  else evaluateCFunction( x, df );
288 
289  for( run1 = 0; run1 < dim; run1++ ){
290  df[run1] = ( df[run1] - f[run1] )/SQRT_EPS;
291  }
292  for( run1 = 0; run1 < nn; run1++ ){
293  x[run1] = x[run1] - SQRT_EPS*seed[run1];
294  }
295  }
296  return SUCCESSFUL_RETURN;
297 }
298 
299 
300 returnValue CFunction::AD_forward( int number, double *x, double *seed, double *f, double *df ){
301 
302  uint run1;
303 
304  if( number >= (int) maxAlloc ){
305 
306  xStore= (double**)realloc(xStore, (number+1)*sizeof(double*));
307  for( run1 = maxAlloc; (int) run1 < number+1; run1++ ){
308  xStore[run1] = new double[nn];
309  }
310  seedStore = (double**)realloc(seedStore, (number+1)*sizeof(double*));
311  for( run1 = maxAlloc; (int) run1 < number+1; run1++ ){
312  seedStore[run1] = new double[nn];
313  }
314  maxAlloc = number+1;
315  }
316 
317  if( cFcnDForward != NULL ){
318  cFcnDForward( number, x, seed, f, df, user_data );
319 
320  for( run1 = 0; run1 < nn; run1++ ){
321 
322  xStore[number][run1] = x [run1];
323  seedStore[number][run1] = seed[run1];
324  }
325  return SUCCESSFUL_RETURN;
326  }
327 
328  if( cFcn != 0 )
329  cFcn( x, f, user_data );
330  else evaluateCFunction( x, f );
331 
332  for( run1 = 0; run1 < nn; run1++ ){
333  xStore[number][run1] = x[run1];
334  seedStore[number][run1] = seed[run1];
335  x [run1] = x[run1] + SQRT_EPS*seed[run1];
336  }
337  if( cFcn != 0 )
338  cFcn( x, df, user_data );
339  else evaluateCFunction( x, df );
340  for( run1 = 0; run1 < dim; run1++ ){
341  df[run1] = ( df[run1] - f[run1] )/SQRT_EPS;
342  }
343  for( run1 = 0; run1 < nn; run1++ ){
344  x[run1] = xStore[number][run1];
345  }
346 
347  return SUCCESSFUL_RETURN;
348 }
349 
350 
351 returnValue CFunction::AD_forward( int number, double *seed, double *df ){
352 
353  uint run1;
354 
355  ASSERT( number < (int) maxAlloc );
356 
357  double *f = new double[dim];
358 
359  if( cFcnDForward != NULL ){
360  cFcnDForward( number, xStore[number], seed, f, df, user_data );
361 
362  for( run1 = 0; run1 < nn; run1++ )
363  seedStore[number][run1] = seed[run1];
364 
365  delete[] f;
366  return SUCCESSFUL_RETURN;
367  }
368 
369  if( cFcn != 0 )
370  cFcn( xStore[number], f, user_data );
371  else evaluateCFunction( xStore[number], f );
372 
373  for( run1 = 0; run1 < nn; run1++ ){
374  xStore[number][run1] = xStore[number][run1] + SQRT_EPS*seed[run1];
375  }
376 
377  if( cFcn != 0 )
378  cFcn( xStore[number], df, user_data );
379  else evaluateCFunction( xStore[number], df );
380  for( run1 = 0; run1 < dim; run1++ ){
381  df[run1] = ( df[run1] - f[run1] )/SQRT_EPS;
382  }
383  for( run1 = 0; run1 < nn; run1++ ){
384  xStore[number][run1] = xStore[number][run1] - SQRT_EPS*seed[run1];
385  }
386 
387  for( run1 = 0; run1 < nn; run1++ ){
388  seedStore[number][run1] = seed[run1];
389  }
390 
391  delete[] f;
392 
393  return SUCCESSFUL_RETURN;
394 }
395 
396 
397 
398 returnValue CFunction::AD_backward( double *seed, double *df ){
399 
400  return AD_backward( 0, seed, df );
401 }
402 
403 
404 returnValue CFunction::AD_backward( int number, double *seed, double *df ){
405 
406  uint run1;
407 
408  ASSERT( number < (int) maxAlloc );
409 
410  if( cFcnDBackward != NULL ){
411 
412  double *f = new double[dim];
413 
414  cFcnDBackward( number, xStore[number], seed, f, df, user_data );
415 
416  for( run1 = 0; run1 < nn; run1++ )
417  seedStore[number][run1] = seed[run1];
418 
419  delete[] f;
420  return SUCCESSFUL_RETURN;
421  }
423 }
424 
425 
426 returnValue CFunction::AD_forward2( int number, double *seed, double *dseed, double *df, double *ddf ){
427 
428  uint run1;
429 
430  ASSERT( number < (int) maxAlloc );
431 
432  double *f1 = new double[dim];
433  double *f2 = new double[dim];
434  double *f3 = new double[dim];
435  double *f4 = new double[dim];
436 
437 
438  // FIRST ORDER DERIVATIVES:
439  // ------------------------
440 
441  evaluate( xStore[number], f1 );
442 
443  for( run1 = 0; run1 < nn; run1++ ){
444  xStore[number][run1] = xStore[number][run1] + SQRT_EPS*seed[run1];
445  }
446  evaluate( xStore[number], df );
447  for( run1 = 0; run1 < dim; run1++ ){
448  df[run1] = ( df[run1] - f1[run1] )/SQRT_EPS;
449  }
450  for( run1 = 0; run1 < nn; run1++ ){
451  xStore[number][run1] = xStore[number][run1] - SQRT_EPS*seed[run1];
452  }
453 
454  for( run1 = 0; run1 < nn; run1++ ){
455  xStore[number][run1] = xStore[number][run1] + SQRT_EPS*dseed[run1];
456  }
457  evaluate( xStore[number], ddf );
458  for( run1 = 0; run1 < dim; run1++ ){
459  ddf[run1] = ( ddf[run1] - f1[run1] )/SQRT_EPS;
460  }
461  for( run1 = 0; run1 < nn; run1++ ){
462  xStore[number][run1] = xStore[number][run1] - SQRT_EPS*dseed[run1];
463  }
464 
465 
466  // SECOND DERIVATIVES:
467  // -------------------
468 
469  for( run1 = 0; run1 < nn; run1++ ){
470  xStore[number][run1] = xStore[number][run1]
471  + 0.5*FOURTH_ROOT_EPS*(seed[run1]+seedStore[number][run1]);
472  }
473  evaluate( xStore[number], f1 );
474 
475  for( run1 = 0; run1 < nn; run1++ ){
476  xStore[number][run1] = xStore[number][run1]
477  - 0.5*FOURTH_ROOT_EPS*(seed[run1]+seedStore[number][run1])
478  + 0.5*FOURTH_ROOT_EPS*(seed[run1]-seedStore[number][run1]);
479  }
480  evaluate( xStore[number], f2 );
481 
482  for( run1 = 0; run1 < nn; run1++ ){
483  xStore[number][run1] = xStore[number][run1]
484  - FOURTH_ROOT_EPS*(seed[run1]-seedStore[number][run1]);
485  }
486  evaluate( xStore[number], f3 );
487 
488  for( run1 = 0; run1 < nn; run1++ ){
489  xStore[number][run1] = xStore[number][run1]
490  + 0.5*FOURTH_ROOT_EPS*(seed[run1]-seedStore[number][run1])
491  - 0.5*FOURTH_ROOT_EPS*(seed[run1]+seedStore[number][run1]);
492  }
493  evaluate( xStore[number], f4 );
494 
495  for( run1 = 0; run1 < nn; run1++ ){
496  xStore[number][run1] = xStore[number][run1]
497  + 0.5*FOURTH_ROOT_EPS*(seed[run1]+seedStore[number][run1]);
498  }
499 
500  for( run1 = 0; run1 < dim; run1++ ){
501  ddf[run1] = ddf[run1] + ( f1[run1] - f2[run1] - f3[run1] + f4[run1] )/SQRT_EPS;
502  }
503 
504  delete[] f1;
505  delete[] f2;
506  delete[] f3;
507  delete[] f4;
508 
509  return SUCCESSFUL_RETURN;
510 }
511 
512 
513 returnValue CFunction::AD_backward2( int number, double *seed1, double *seed2, double *df, double *ddf ){
514 
516 }
517 
519 
520  uint run1;
521 
522  if( maxAlloc > 1 ){
523 
524  for( run1 = 1; run1 < maxAlloc; run1++ ){
525  delete[] xStore[run1];
526  delete[] seedStore[run1];
527  }
528 
529  maxAlloc = 1;
530 
531  xStore= (double**)realloc( xStore, maxAlloc*sizeof(double*) );
532  seedStore = (double**)realloc( seedStore, maxAlloc*sizeof(double*) );
533  }
534 
535  return SUCCESSFUL_RETURN;
536 }
537 
538 
540 
541  user_data = user_data_;
542 
543  return SUCCESSFUL_RETURN;
544 }
545 
546 
547 
549 
550 // end of file.
const double SQRT_EPS
virtual returnValue AD_backward2(int number, double *seed1, double *seed2, double *df, double *ddf)
Definition: c_function.cpp:513
Allows to pass back messages to the calling function.
virtual void evaluateCFunction(double *x, double *result)
Definition: c_function.cpp:215
virtual returnValue AD_forward2(int number, double *seed1, double *seed2, double *df, double *ddf)
Definition: c_function.cpp:426
returnValue initialize()
Definition: c_function.cpp:103
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
#define CLOSE_NAMESPACE_ACADO
void(* cFcnDPtr)(int number, double *x, double *seed, double *f, double *df, void *userData)
Definition: acado_types.hpp:59
Base class for all variables within the symbolic expressions family.
Definition: expression.hpp:56
double ** xStore
Definition: c_function.hpp:301
Operator ** element
Definition: expression.hpp:311
void * user_data
Definition: c_function.hpp:294
void copy(const CFunction &arg)
Definition: c_function.cpp:118
virtual Expression operator()(const Expression &arg)
Definition: c_function.cpp:169
virtual ~CFunction()
Definition: c_function.cpp:91
const double FOURTH_ROOT_EPS
virtual returnValue evaluate(double *x, double *result)
Definition: c_function.cpp:205
cFcnDPtr cFcnDForward
Definition: c_function.hpp:290
virtual uint getDim() const
Definition: c_function.cpp:199
virtual returnValue setUserData(void *user_data_)
Definition: c_function.cpp:539
The class COperator is an auxiliary class to use C-Functions within a function evaluation tree...
Definition: c_operator.hpp:58
PrintLevel
CFunction & operator=(const CFunction &rhs)
Definition: c_function.cpp:93
#define ASSERT(x)
virtual returnValue AD_backward(double *seed, double *df)
Definition: c_function.cpp:398
cFcnDPtr cFcnDBackward
Definition: c_function.hpp:291
uint maxAlloc
Definition: c_function.hpp:300
(no description yet)
Definition: c_function.hpp:54
void deleteAll()
Definition: c_function.cpp:156
int increaseID()
Definition: c_operator.cpp:116
#define BEGIN_NAMESPACE_ACADO
void(* cFcnPtr)(double *x, double *f, void *userData)
Definition: acado_types.hpp:56
virtual returnValue clearBuffer()
Definition: c_function.cpp:518
double ** seedStore
Definition: c_function.hpp:302
cFcnPtr cFcn
Definition: c_function.hpp:289
virtual returnValue AD_forward(double *x, double *seed, double *f, double *df)
Definition: c_function.cpp:267
#define ACADOERROR(retval)
uint getDim() const


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:34:29