1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/base/drive.cc,v 1.60 2017/01/12 14:46:09 masarati Exp $ */
2 /*
3 * MBDyn (C) is a multibody analysis code.
4 * http://www.mbdyn.org
5 *
6 * Copyright (C) 1996-2017
7 *
8 * Pierangelo Masarati <masarati@aero.polimi.it>
9 * Paolo Mantegazza <mantegazza@aero.polimi.it>
10 *
11 * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
12 * via La Masa, 34 - 20156 Milano, Italy
13 * http://www.aero.polimi.it
14 *
15 * Changing this copyright notice is forbidden.
16 *
17 * This program is free software; you can redistribute it and/or modify
18 * it under the terms of the GNU General Public License as published by
19 * the Free Software Foundation (version 2 of the License).
20 *
21 *
22 * This program is distributed in the hope that it will be useful,
23 * but WITHOUT ANY WARRANTY; without even the implied warranty of
24 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
25 * GNU General Public License for more details.
26 *
27 * You should have received a copy of the GNU General Public License
28 * along with this program; if not, write to the Free Software
29 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30 */
31
32 /* drivers */
33
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35
36 #include "drive.h"
37
38 doublereal Drive::dReturnValue = 0.;
39 doublereal DriveHandler::dDriveHandlerReturnValue = 0.;
40
41 /* Drive - begin */
42
Drive(unsigned int uL,const DriveHandler * pDH)43 Drive::Drive(unsigned int uL, const DriveHandler* pDH)
44 : WithLabel(uL), pDrvHdl(pDH)
45 {
46 NO_OP;
47 }
48
49
~Drive(void)50 Drive::~Drive(void) {
51 NO_OP;
52 }
53
54 /* Drive - end */
55
56
57 /* DriveHandler - begin */
58
DriveHandler(MathParser & mp)59 DriveHandler::DriveHandler(MathParser& mp)
60 : Parser(mp),
61 pTime(0),
62 pTimeStep(0),
63 pStep(0),
64 pVar(0),
65 pXCurr(0),
66 pXPrimeCurr(0),
67 iCurrStep(0),
68 Meter(0),
69 Rand(0),
70 ClosestNext(0),
71 SH(0)
72 {
73 #ifdef USE_MULTITHREAD
74 pthread_mutexattr_t ma;
75 pthread_mutexattr_init(&ma);
76 pthread_mutexattr_settype(&ma, PTHREAD_MUTEX_RECURSIVE);
77 int rc = pthread_mutex_init(&parser_mutex, &ma);
78 pthread_mutexattr_destroy(&ma);
79 if (rc) {
80 silent_cerr("DriveHandler::DriveHandler(): mutex init failed"
81 << std::endl);
82 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
83 }
84 #endif /* USE_MULTITHREAD */
85
86 NamedValue *v;
87
88 /* Inserisce la variabile Time nella tabella dei simboli; sara'
89 * mantenuta aggiornata dal DriveHandler */
90 v = Parser.GetSymbolTable().Get("Time");
91 if (v == 0) {
92 pTime = Parser.GetSymbolTable().Put("Time", Real(0));
93 if (pTime == 0) {
94 silent_cerr("DriveHandler::DriveHandler(): "
95 "error while inserting symbol 'Time'"
96 << std::endl);
97 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
98 }
99
100 } else {
101 if (!v->IsVar()) {
102 silent_cerr("Symbol 'Time' must be a variable"
103 << std::endl);
104 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
105 }
106 pTime = dynamic_cast<Var *>(v);
107 }
108
109 /* Inserisce la variabile TimeStep nella tabella dei simboli; sara'
110 * mantenuta aggiornata dal DriveHandler */
111 v = Parser.GetSymbolTable().Get("TimeStep");
112 if (v == 0) {
113 pTimeStep = Parser.GetSymbolTable().Put("TimeStep", Real(-1.));
114 if (pTimeStep == 0) {
115 silent_cerr("DriveHandler::DriveHandler(): "
116 "error while inserting symbol 'TimeStep'"
117 << std::endl);
118 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
119 }
120
121 } else {
122 if (!v->IsVar()) {
123 silent_cerr("Symbol 'TimeStep' must be a variable"
124 << std::endl);
125 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
126 }
127 pTimeStep = dynamic_cast<Var *>(v);
128 }
129
130 /* Inserisce la variabile Step nella tabella dei simboli; sara'
131 * mantenuta aggiornata dal DriveHandler */
132 v = Parser.GetSymbolTable().Get("Step");
133 if (v == 0) {
134 pStep = Parser.GetSymbolTable().Put("Step", Int(-1));
135 if (pStep == 0) {
136 silent_cerr("DriveHandler::DriveHandler(): "
137 "error while inserting symbol 'Step'"
138 << std::endl);
139 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
140 }
141
142 } else {
143 if (!v->IsVar()) {
144 silent_cerr("Symbol 'Step' must be a variable"
145 << std::endl);
146 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
147 }
148 pStep = dynamic_cast<Var *>(v);
149 }
150
151 /* Inserisce la variabile Var nella tabella dei simboli; sara'
152 * mantenuta aggiornata dai DriveCaller attraverso il DriveHandler */
153 v = Parser.GetSymbolTable().Get("Var");
154 if (v == 0) {
155 pVar = Parser.GetSymbolTable().Put("Var", Real(0));
156 if (pVar == 0) {
157 silent_cerr("DriveHandler::DriveHandler(): "
158 "error while insterting symbol 'Var'"
159 << std::endl);
160 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
161 }
162
163 } else {
164 if (!v->IsVar()) {
165 silent_cerr("Symbol 'Var' must be a variable"
166 << std::endl);
167 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
168 }
169 pVar = dynamic_cast<Var *>(v);
170 }
171
172 /* Calcola il seed di riferimento per i numeri random */
173 srand(time(NULL));
174 }
175
~DriveHandler(void)176 DriveHandler::~DriveHandler(void)
177 {
178 #ifdef USE_MULTITHREAD
179 pthread_mutex_destroy(&parser_mutex);
180 #endif /* USE_MULTITHREAD */
181
182 for (std::vector<MyMeter *>::iterator i = Meter.begin();
183 i != Meter.end(); ++i)
184 {
185 SAFEDELETE(*i);
186 }
187
188 for (std::vector<MyRand *>::iterator i = Rand.begin();
189 i != Rand.end(); ++i)
190 {
191 SAFEDELETE(*i);
192 }
193
194 for (std::vector<MyClosestNext*>::iterator i = ClosestNext.begin();
195 i != ClosestNext.end(); ++i)
196 {
197 SAFEDELETE(*i);
198 }
199
200 for (std::vector<MySH *>::iterator i = SH.begin();
201 i != SH.end(); ++i)
202 {
203 SAFEDELETE(*i);
204 }
205 }
206
207 void
SetTime(const doublereal & dt,const doublereal & dts,const integer & s)208 DriveHandler::SetTime(const doublereal& dt, const doublereal& dts,
209 const integer& s)
210 {
211 /* Setta la variabile Time nella tabella dei simboli */
212 ASSERT(pTime != 0);
213 pTime->SetVal(dt);
214
215 /* Setta la variabile TimeStep nella tabella dei simboli */
216 if (dts >= 0.) {
217 ASSERT(pTimeStep != 0);
218 pTimeStep->SetVal(dts);
219 }
220
221 /* Setta la variabile Step nella tabella dei simboli */
222 if (s >= 0) {
223 ASSERT(pStep != 0);
224 pStep->SetVal(s);
225
226 /* in case of new step */
227 if (s != iCurrStep) {
228 ASSERT(iCurrStep + 1 == s);
229 iCurrStep = s;
230
231 for (std::vector<MyMeter *>::iterator i = Meter.begin();
232 i != Meter.end(); ++i)
233 {
234 (*i)->Set();
235 }
236
237 for (std::vector<MyRand *>::iterator i = Rand.begin();
238 i != Rand.end(); ++i)
239 {
240 (*i)->Set();
241 }
242
243 for (std::vector<MyClosestNext*>::iterator i = ClosestNext.begin();
244 i != ClosestNext.end(); ++i)
245 {
246 (*i)->Set();
247 }
248
249 for (std::vector<MySH *>::iterator i = SH.begin();
250 i != SH.end(); ++i)
251 {
252 (*i)->Set();
253 }
254 }
255 }
256 }
257
258
259 void
LinkToSolution(const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)260 DriveHandler::LinkToSolution(const VectorHandler& XCurr,
261 const VectorHandler& XPrimeCurr)
262 {
263 pXCurr = const_cast<VectorHandler *>(&XCurr);
264 pXPrimeCurr = const_cast<VectorHandler *>(&XPrimeCurr);
265 }
266
267 integer
iRandInit(integer iSteps)268 DriveHandler::iRandInit(integer iSteps)
269 {
270 MyRand* pmr = 0;
271 integer iNumber = Rand.size();
272 SAFENEWWITHCONSTRUCTOR(pmr,
273 MyRand,
274 MyRand((unsigned int)iNumber, iSteps, rand()));
275 Rand.push_back(pmr);
276
277 return iNumber;
278 }
279
280 integer
iMeterInit(integer iSteps)281 DriveHandler::iMeterInit(integer iSteps)
282 {
283 MyMeter* pmm = 0;
284 integer iNumber = Meter.size();
285 SAFENEWWITHCONSTRUCTOR(pmm,
286 MyMeter,
287 MyMeter((unsigned int)iNumber, iSteps));
288 Meter.push_back(pmm);
289
290 return iNumber;
291 }
292
293 integer
iClosestNextInit(const DriveCaller * pIncrement,doublereal dStartTime)294 DriveHandler::iClosestNextInit(const DriveCaller *pIncrement,
295 doublereal dStartTime)
296 {
297 MyClosestNext* pmc = 0;
298 integer iNumber = ClosestNext.size();
299 SAFENEWWITHCONSTRUCTOR(pmc,
300 MyClosestNext,
301 MyClosestNext((unsigned int)iNumber, this,
302 pIncrement, dStartTime));
303 ClosestNext.push_back(pmc);
304
305 return iNumber;
306 }
307
308
309 integer
iSHInit(const DriveCaller * pFunc,const DriveCaller * pTrigger,const doublereal dVal0)310 DriveHandler::iSHInit(const DriveCaller *pFunc, const DriveCaller *pTrigger,
311 const doublereal dVal0)
312 {
313 MySH* pms = 0;
314 integer iNumber = SH.size();
315 SAFENEWWITHCONSTRUCTOR(pms,
316 MySH,
317 MySH((unsigned int)iNumber, pFunc, pTrigger, dVal0));
318 SH.push_back(pms);
319
320 return iNumber;
321 }
322
323 const DriveCaller *
pGetSHFunc(integer iNumber) const324 DriveHandler::pGetSHFunc(integer iNumber) const
325 {
326 return SH[iNumber]->pGetFunc();
327 }
328
329 const DriveCaller *
pGetSHTrigger(integer iNumber) const330 DriveHandler::pGetSHTrigger(integer iNumber) const
331 {
332 return SH[iNumber]->pGetTrigger();
333 }
334
335 const doublereal
dGetSHVal0(integer iNumber) const336 DriveHandler::dGetSHVal0(integer iNumber) const
337 {
338 return SH[iNumber]->dGetVal0();
339 }
340
341 void
PutSymbolTable(Table & T)342 DriveHandler::PutSymbolTable(Table& T)
343 {
344 Parser.PutSymbolTable(T);
345 }
346
347
348 void
SetVar(const doublereal & dVar)349 DriveHandler::SetVar(const doublereal& dVar)
350 {
351 ASSERT(pVar != 0);
352 pVar->SetVal(dVar);
353 }
354
355
356 #ifndef USE_EE
357 doublereal
dGet(InputStream & InStr) const358 DriveHandler::dGet(InputStream& InStr) const
359 {
360 doublereal d;
361
362 #ifdef USE_MULTITHREAD
363 // FIXME: risk recursive lock
364 pthread_mutex_lock(&parser_mutex);
365 #endif /* USE_MULTITHREAD */
366
367 try {
368 d = Parser.GetLastStmt(InStr);
369
370 } catch (MBDynErrBase& e) {
371 silent_cerr("StringDrive: " << e.what() << std::endl);
372
373 #ifdef USE_MULTITHREAD
374 pthread_mutex_unlock(&parser_mutex);
375 #endif /* USE_MULTITHREAD */
376
377 throw e;
378
379 #if 0
380 } catch (ErrGeneric e) {
381 silent_cerr("StringDrive: " << e.what() << std::endl);
382 throw e;
383 #endif
384
385 } catch (...) {
386 silent_cerr("StringDrive generic error" << std::endl);
387
388 #ifdef USE_MULTITHREAD
389 pthread_mutex_unlock(&parser_mutex);
390 #endif /* USE_MULTITHREAD */
391
392 throw;
393 }
394
395 #ifdef USE_MULTITHREAD
396 pthread_mutex_unlock(&parser_mutex);
397 #endif /* USE_MULTITHREAD */
398
399 return d;
400 }
401 #endif // ! USE_EE
402
MyRand(unsigned int uLabel,integer iS,integer iR)403 DriveHandler::MyRand::MyRand(unsigned int uLabel, integer iS, integer iR)
404 : MyMeter(uLabel, iS), iRand(iR)
405 {
406 NO_OP;
407 }
408
~MyRand(void)409 DriveHandler::MyRand::~MyRand(void)
410 {
411 NO_OP;
412 }
413
MyMeter(unsigned int uLabel,integer iS)414 DriveHandler::MyMeter::MyMeter(unsigned int uLabel, integer iS)
415 : WithLabel(uLabel), iSteps(iS), iCurr(0)
416 {
417 NO_OP;
418 }
419
~MyMeter(void)420 DriveHandler::MyMeter::~MyMeter(void)
421 {
422 NO_OP;
423 }
424
MyClosestNext(unsigned int uLabel,const DriveHandler * pDH,const DriveCaller * pIncrement,doublereal dStartTime)425 DriveHandler::MyClosestNext::MyClosestNext(unsigned int uLabel,
426 const DriveHandler *pDH,
427 const DriveCaller *pIncrement,
428 doublereal dStartTime)
429 : WithLabel(uLabel), pDH(pDH), Increment(pIncrement), bMustSetNext(false), dNext(dStartTime)
430 {
431 NO_OP;
432 }
433
~MyClosestNext(void)434 DriveHandler::MyClosestNext::~MyClosestNext(void)
435 {
436 NO_OP;
437 }
438
MySH(unsigned int uLabel,const DriveCaller * pFunc,const DriveCaller * pTrigger,const doublereal dVal0)439 DriveHandler::MySH::MySH(unsigned int uLabel,
440 const DriveCaller *pFunc,
441 const DriveCaller *pTrigger,
442 const doublereal dVal0)
443 : WithLabel(uLabel), dVal0(dVal0), dVal(dVal0), Func(pFunc), Trigger(pTrigger)
444 {
445 NO_OP;
446 }
447
~MySH(void)448 DriveHandler::MySH::~MySH(void)
449 {
450 NO_OP;
451 }
452
453 const DriveCaller *
pGetFunc(void) const454 DriveHandler::MySH::pGetFunc(void) const
455 {
456 return Func.pGetDriveCaller();
457 }
458
459 const DriveCaller *
pGetTrigger(void) const460 DriveHandler::MySH::pGetTrigger(void) const
461 {
462 return Trigger.pGetDriveCaller();
463 }
464
465 const doublereal
dGetVal0(void) const466 DriveHandler::MySH::dGetVal0(void) const
467 {
468 return dVal0;
469 }
470
471 /* DriveHandler - end */
472
473 /* DriveCaller - begin */
474
DriveCaller(const DriveHandler * pDH)475 DriveCaller::DriveCaller(const DriveHandler* pDH)
476 : pDrvHdl(const_cast<DriveHandler *>(pDH))
477 {
478 NO_OP;
479 }
480
~DriveCaller(void)481 DriveCaller::~DriveCaller(void)
482 {
483 NO_OP;
484 }
485
486 void
SetDrvHdl(const DriveHandler * pDH)487 DriveCaller::SetDrvHdl(const DriveHandler* pDH)
488 {
489 pDrvHdl = const_cast<DriveHandler *>(pDH);
490 }
491
492 const DriveHandler *
pGetDrvHdl(void) const493 DriveCaller::pGetDrvHdl(void) const
494 {
495 return pDrvHdl;
496 }
497
498 doublereal
dGetP(const doublereal & dVar) const499 DriveCaller::dGetP(const doublereal& dVar) const
500 {
501 /* shouldn't get called if not differentiable,
502 * or should be overridden if differentiable */
503 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
504 }
505
Output(OutputHandler & OH) const506 void DriveCaller::Output(OutputHandler& OH) const
507 {
508 const flag fOut = fToBeOutput();
509
510 if (fOut & flag(1)) {
511 if (OH.UseText(OutputHandler::DRIVECALLERS)) {
512 std::ostream& os = OH.DriveCallers();
513
514 os << GetLabel();
515
516 if (fOut & OUTPUT_VALUE) {
517 os << ' ' << dGet();
518 }
519
520 if (fOut & OUTPUT_DERIVATIVE) {
521 ASSERT(bIsDifferentiable());
522
523 os << ' ' << dGetP();
524 }
525
526 os << std::endl;
527 }
528 }
529 }
530
Trace(OutputHandler & OH) const531 void DriveCaller::Trace(OutputHandler& OH) const
532 {
533 const flag fTrace = fToBeTraced();
534
535 if (fTrace) {
536 if (OH.UseText(OutputHandler::TRACES)) {
537 std::ostream& os = OH.Traces();
538
539 os << GetLabel();
540
541 if (fTrace & TRACE_VALUE) {
542 os << ' ' << dGet();
543 }
544
545 if (fTrace & TRACE_DERIVATIVE) {
546 ASSERT(bIsDifferentiable());
547
548 os << ' ' << dGetP();
549 }
550
551 os << std::endl;
552 }
553 }
554 }
555
556 /* DriveCaller - end */
557
558
559 /* NullDriveCaller - begin */
560
NullDriveCaller(void)561 NullDriveCaller::NullDriveCaller(void)
562 : DriveCaller(0)
563 {
564 NO_OP;
565 }
566
~NullDriveCaller(void)567 NullDriveCaller::~NullDriveCaller(void)
568 {
569 NO_OP;
570 }
571
572 /* Copia */
573 DriveCaller *
pCopy(void) const574 NullDriveCaller::pCopy(void) const
575 {
576 DriveCaller* pDC = 0;
577 SAFENEW(pDC, NullDriveCaller);
578
579 return pDC;
580 }
581
582 /* Scrive il contributo del DriveCaller al file di restart */
583 std::ostream&
Restart(std::ostream & out) const584 NullDriveCaller::Restart(std::ostream& out) const
585 {
586 return out << "null";
587 }
588
589 /* NullDriveCaller - end */
590
591
592 /* OneDriveCaller - begin */
593
OneDriveCaller(void)594 OneDriveCaller::OneDriveCaller(void)
595 : DriveCaller(0)
596 {
597 NO_OP;
598 }
599
~OneDriveCaller(void)600 OneDriveCaller::~OneDriveCaller(void)
601 {
602 NO_OP;
603 }
604
605 /* Copia */
606 DriveCaller *
pCopy(void) const607 OneDriveCaller::pCopy(void) const
608 {
609 DriveCaller* pDC = 0;
610 SAFENEW(pDC, OneDriveCaller);
611
612 return pDC;
613 }
614
615 /* Scrive il contributo del DriveCaller al file di restart */
616 std::ostream&
Restart(std::ostream & out) const617 OneDriveCaller::Restart(std::ostream& out) const
618 {
619 return out << "one";
620 }
621
622 /* OneDriveCaller - end */
623
624
625 /* DriveOwner - begin */
626
DriveOwner(const DriveCaller * pDC)627 DriveOwner::DriveOwner(const DriveCaller* pDC)
628 : pDriveCaller(const_cast<DriveCaller*>(pDC))
629 {
630 NO_OP;
631 }
632
DriveOwner(const DriveOwner & drive)633 DriveOwner::DriveOwner(const DriveOwner& drive)
634 : pDriveCaller(drive.pDriveCaller ? drive.pDriveCaller->pCopy() : 0)
635 {
636 NO_OP;
637 }
638
~DriveOwner(void)639 DriveOwner::~DriveOwner(void)
640 {
641 if (pDriveCaller != 0) {
642 SAFEDELETE(pDriveCaller);
643 }
644 }
645
646 void
Set(const DriveCaller * pDC)647 DriveOwner::Set(const DriveCaller* pDC)
648 {
649 ASSERT(pDC != 0);
650 if (pDriveCaller != 0) {
651 DEBUGCOUT("warning: the original pointer to a drive caller is not null!" << std::endl);
652 SAFEDELETE(pDriveCaller);
653 }
654 pDriveCaller = const_cast<DriveCaller*>(pDC);
655 }
656
657 DriveCaller *
pGetDriveCaller(void) const658 DriveOwner::pGetDriveCaller(void) const
659 {
660 return pDriveCaller;
661 }
662
663 doublereal
dGet(const doublereal & dVal) const664 DriveOwner::dGet(const doublereal& dVal) const
665 {
666 return pDriveCaller->dGet(dVal);
667 }
668
669
670 doublereal
dGet(void) const671 DriveOwner::dGet(void) const
672 {
673 return pDriveCaller->dGet();
674 }
675
676 bool
bIsDifferentiable(void) const677 DriveOwner::bIsDifferentiable(void) const
678 {
679 return pDriveCaller->bIsDifferentiable();
680 }
681
682 doublereal
dGetP(const doublereal & dVal) const683 DriveOwner::dGetP(const doublereal& dVal) const
684 {
685 return pDriveCaller->dGetP(dVal);
686 }
687
688
689 doublereal
dGetP(void) const690 DriveOwner::dGetP(void) const
691 {
692 return pDriveCaller->dGetP();
693 }
694
695 /* DriveOwner - end */
696
697