1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/base/mbpar.cc,v 1.114 2017/08/28 10:34:39 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 /* parser */
33
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35
36 #include <limits>
37 #include <cfloat>
38 #include <limits>
39 #include <typeinfo>
40
41 #if defined(USE_RUNTIME_LOADING) && defined(HAVE_LTDL_H)
42 #include <ltdl.h>
43 #endif // USE_RUNTIME_LOADING && HAVE_LTDL_H
44
45 #include "mbpar.h"
46 #include "Rot.hh"
47
48 #include "hfluid.h"
49
50 #include "aerodc81.h"
51 #include "c81data.h"
52
53 #include "dataman.h"
54 #include "modules.h"
55
56 #include "legalese.h"
57 #include "streamoutelem.h"
58 #include "socketstreamdrive.h"
59 #include "drive_.h"
60
61 struct RefFrameDR : public DescRead {
62 bool Read(HighParser& HP);
63 };
64
65 bool
Read(HighParser & HP)66 RefFrameDR::Read(HighParser& HP)
67 {
68 MBDynParser *pMBP = dynamic_cast<MBDynParser *>(&HP);
69 ASSERT(pMBP != 0);
70 return pMBP->Reference_int();
71 }
72
73 struct HFluidDR : public DescRead {
74 bool Read(HighParser& HP);
75 };
76
77 bool
Read(HighParser & HP)78 HFluidDR::Read(HighParser& HP)
79 {
80 MBDynParser *pMBP = dynamic_cast<MBDynParser *>(&HP);
81 ASSERT(pMBP != 0);
82 return pMBP->HydraulicFluid_int();
83 }
84
85 struct C81DataDR : public DescRead {
86 bool Read(HighParser& HP);
87 };
88
89 bool
Read(HighParser & HP)90 C81DataDR::Read(HighParser& HP)
91 {
92 MBDynParser *pMBP = dynamic_cast<MBDynParser *>(&HP);
93 ASSERT(pMBP != 0);
94 return pMBP->C81Data_int();
95 }
96
97 struct ConstLawDR : public DescRead {
98 bool Read(HighParser& HP);
99 };
100
101 bool
Read(HighParser & HP)102 ConstLawDR::Read(HighParser& HP)
103 {
104 MBDynParser *pMBP = dynamic_cast<MBDynParser *>(&HP);
105 ASSERT(pMBP != 0);
106 return pMBP->ConstitutiveLaw_int();
107 }
108
109 struct DriveCallerDR : public DescRead {
110 bool Read(HighParser& HP);
111 };
112
113 bool
Read(HighParser & HP)114 DriveCallerDR::Read(HighParser& HP)
115 {
116 MBDynParser *pMBP = dynamic_cast<MBDynParser *>(&HP);
117 ASSERT(pMBP != 0);
118 return pMBP->DriveCaller_int();
119 }
120
121 struct TplDriveCallerDR : public DescRead {
122 bool Read(HighParser& HP);
123 };
124
125 bool
Read(HighParser & HP)126 TplDriveCallerDR::Read(HighParser& HP)
127 {
128 MBDynParser *pMBP = dynamic_cast<MBDynParser *>(&HP);
129 ASSERT(pMBP != 0);
130 return pMBP->TplDriveCaller_int();
131 }
132
133 struct SFuncDR : public DescRead {
134 bool Read(HighParser& HP);
135 };
136
137 bool
Read(HighParser & HP)138 SFuncDR::Read(HighParser& HP)
139 {
140 MBDynParser *pMBP = dynamic_cast<MBDynParser *>(&HP);
141 ASSERT(pMBP != 0);
142 return pMBP->ScalarFunction_int();
143 }
144
145 struct ModuleLoadDR : public DescRead {
146 bool Read(HighParser& HP);
147 };
148
149 bool
Read(HighParser & HP)150 ModuleLoadDR::Read(HighParser& HP)
151 {
152 MBDynParser *pMBP = dynamic_cast<MBDynParser *>(&HP);
153 ASSERT(pMBP != 0);
154 return pMBP->ModuleLoad_int();
155 }
156
157 struct LicenseDR : public DescRead {
158 bool Read(HighParser& HP);
159 };
160
161 bool
Read(HighParser & HP)162 LicenseDR::Read(HighParser& HP)
163 {
164 mbdyn_license();
165 return true;
166 }
167
168 struct WarrantyDR : public DescRead {
169 bool Read(HighParser& HP);
170 };
171
172 bool
Read(HighParser & HP)173 WarrantyDR::Read(HighParser& HP)
174 {
175 mbdyn_warranty();
176 return true;
177 }
178
179 static unsigned desc_done;
180
181 static void
InitDescData(void)182 InitDescData(void)
183 {
184 // NOTE: data will be destroyed when the underlying HighParser is destroyed (is this what we want?)
185 if (::desc_done++ > 0) {
186 return;
187 }
188
189 SetDescData("reference", new RefFrameDR);
190 SetDescData("hydraulic" "fluid", new HFluidDR);
191 SetDescData("c81" "data", new C81DataDR);
192 SetDescData("constitutive" "law", new ConstLawDR);
193 SetDescData("drive" "caller", new DriveCallerDR);
194 SetDescData("template" "drive" "caller", new TplDriveCallerDR);
195 SetDescData("scalar" "function", new SFuncDR);
196 SetDescData("module" "load", new ModuleLoadDR);
197 // TODO: move to HighParser
198 SetDescData("license", new LicenseDR);
199 SetDescData("warranty", new WarrantyDR);
200
201 /* NOTE: add here initialization of new built-in descriptions;
202 * alternative ways to register new custom descriptions are:
203 * - call SetDescData() from anywhere in the code
204 * - write a module that calls SetDescData() from inside a function
205 * called module_init(), and run-time load it using "module load"
206 * in the input file.
207 */
208 }
209
210
211
212 /* MBDynParser - begin */
213
MBDynParser(MathParser & MP,InputStream & streamIn,const char * initial_file)214 MBDynParser::MBDynParser(MathParser& MP,
215 InputStream& streamIn,
216 const char *initial_file)
217 : IncludeParser(MP, streamIn, initial_file),
218 moduleInitialized(false),
219 pDM(0)
220 {
221 /* make sure this is init'ed */
222 InitDriveCallerData();
223 InitTplDC();
224 InitCL();
225 InitSF();
226 InitStreamOutputContentTypes();
227
228 // NOTE: data will be destroyed when the underlying HighParser is destroyed (is this what we want?)
229 InitDescData();
230 }
231
~MBDynParser(void)232 MBDynParser::~MBDynParser(void)
233 {
234 DestroyDriveCallerData();
235 DestroyTplDC();
236 DestroyCL();
237 DestroySF();
238 DestroyStreamOutputContentTypes();
239 DestroyFileDriveContentTypes();
240 DestroyFileDriveCallerTypes();
241
242 for (SFType::iterator i = SF.begin(); i != SF.end(); ++i) {
243 SAFEDELETE(i->second);
244 }
245 SF.clear();
246
247 for (RFType::iterator i = RF.begin(); i != RF.end(); ++i) {
248 SAFEDELETE(i->second);
249 }
250 RF.clear();
251
252 for (HFType::iterator i = HF.begin(); i != HF.end(); ++i) {
253 SAFEDELETE(i->second);
254 }
255 HF.clear();
256
257 for (ADType::iterator i = AD.begin(); i != AD.end(); ++i) {
258 c81_data_destroy(i->second);
259 SAFEDELETE(i->second);
260 }
261 AD.clear();
262
263 for (CL1DType::iterator i = CL1D.begin(); i != CL1D.end(); ++i) {
264 SAFEDELETE(i->second);
265 }
266 CL1D.clear();
267
268 for (CL3DType::iterator i = CL3D.begin(); i != CL3D.end(); ++i) {
269 SAFEDELETE(i->second);
270 }
271 CL3D.clear();
272
273 for (CL6DType::iterator i = CL6D.begin(); i != CL6D.end(); ++i) {
274 SAFEDELETE(i->second);
275 }
276 CL6D.clear();
277
278 for (DCType::iterator i = DC.begin(); i != DC.end(); ++i) {
279 SAFEDELETE(i->second);
280 }
281 DC.clear();
282
283 if (!bEmptyManip()) {
284 silent_cerr("MBDynParser::~MBDynParser: "
285 "manipulators' stack not empty" << std::endl);
286 }
287
288 #if defined(USE_RUNTIME_LOADING)
289 if (moduleInitialized) {
290 module_finalize();
291 }
292 #endif // USE_RUNTIME_LOADING
293 }
294
295 void
SetDataManager(DataManager * pdm)296 MBDynParser::SetDataManager(DataManager *pdm)
297 {
298 ASSERT(pdm != NULL);
299 pDM = pdm;
300 const DriveHandler *pDH = pDM->pGetDrvHdl();
301 if (pDH == 0) {
302 silent_cerr("no drive handler is associated to data manager?"
303 << std::endl);
304
305 } else {
306 /* add the drive handler to the drive callers... */
307 for (DCType::const_iterator i = DC.begin(); i != DC.end(); ++i) {
308 const_cast<DriveCaller *>(i->second)->SetDrvHdl(pDH);
309 }
310 }
311 }
312
313 DataManager *
GetDataManager(void) const314 MBDynParser::GetDataManager(void) const
315 {
316 return pDM;
317 }
318
319 const ReferenceFrame AbsRefFrame(0,
320 Vec3(0., 0., 0),
321 Mat3x3(1., 0., 0., 0., 1., 0., 0., 0., 1.),
322 Vec3(0., 0., 0),
323 Vec3(0., 0., 0),
324 EULER_123);
325
326 bool
Reference_int(void)327 MBDynParser::Reference_int(void)
328 {
329 if (!IsArg()) {
330 silent_cerr("Parser error in MBDynParser::Reference_int(),"
331 " arg expected at line "
332 << GetLineData() << std::endl);
333 throw HighParser::ErrColonExpected(MBDYN_EXCEPT_ARGS);
334 }
335
336 unsigned int uLabel(GetInt());
337
338 /* Nome del reference */
339 std::string sName;
340 if (IsKeyWord("name")) {
341 const char *sTmp = GetStringWithDelims();
342 sName = sTmp;
343 }
344
345 DEBUGLCOUT(MYDEBUG_INPUT, "Reference frame " << uLabel << std::endl);
346
347 Vec3 x(::Zero3);
348 Mat3x3 R(::Zero3x3);
349 Vec3 v(::Zero3);
350 Vec3 w(::Zero3);
351
352 if (IsKeyWord("denavit" "hartenberg")) {
353 ReferenceFrame rfOut;
354 switch (GetRef(rfOut)) {
355 case MBDynParser::UNKNOWNFRAME:
356 case MBDynParser::GLOBAL:
357 rfOut = ::AbsRefFrame;
358 break;
359
360 case MBDynParser::REFERENCE:
361 break;
362
363 default:
364 silent_cerr("ReferenceFrame(" << uLabel << "): invalid reference specification at line " << GetLineData() << std::endl);
365 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
366 }
367
368 doublereal d = GetReal();
369 doublereal theta = GetReal();
370 doublereal a = GetReal();
371 doublereal alpha = GetReal();
372
373 Mat3x3 Rtheta(RotManip::Rot(Vec3(0., 0., theta)));
374 Mat3x3 Ralpha(RotManip::Rot(Vec3(alpha, 0., 0.)));
375 Vec3 Xr = rfOut.GetR()*(Vec3(0., 0., d) + Rtheta*Vec3(a, 0., 0.));
376 x = rfOut.GetX() + Xr;
377 R = rfOut.GetR()*Rtheta*Ralpha;
378 v = rfOut.GetV() + rfOut.GetW().Cross(Xr);
379 w = rfOut.GetW();
380
381 } else {
382 if (!IsKeyWord("position")) {
383 pedantic_cerr("ReferenceFrame(" << uLabel
384 << "): missing keyword \"position\" at line "
385 << GetLineData() << std::endl);
386 }
387 x = GetPosAbs(::AbsRefFrame);
388
389 if (!IsKeyWord("orientation")) {
390 pedantic_cerr("ReferenceFrame(" << uLabel
391 << "): missing keyword \"orientation\" at line "
392 << GetLineData() << std::endl);
393 }
394 R = GetRotAbs(::AbsRefFrame);
395
396 if (IsArg()) {
397 if (!IsKeyWord("velocity")) {
398 pedantic_cerr("ReferenceFrame(" << uLabel
399 << "): missing keyword \"velocity\" at line "
400 << GetLineData() << std::endl);
401 }
402 v = GetVelAbs(::AbsRefFrame, x);
403 if (IsArg()) {
404 if (!IsKeyWord("angular" "velocity")) {
405 pedantic_cerr("ReferenceFrame(" << uLabel
406 << "): missing keyword \"angular velocity\" at line "
407 << GetLineData() << std::endl);
408 }
409 w = GetOmeAbs(::AbsRefFrame);
410 }
411 }
412 }
413
414 if (IsArg()) {
415 silent_cerr("semicolon expected after reference " << uLabel
416 << " (" << (sName.empty() ? "unknown" : sName.c_str()) << ") "
417 "at line " << GetLineData() << std::endl);
418 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
419 }
420
421 DEBUGLCOUT(MYDEBUG_INPUT, std::endl
422 << "\tX = " << x << std::endl
423 << "\tR = " << R << std::endl
424 << "\tV = " << v << std::endl
425 << "\tW = " << w << std::endl);
426
427 ReferenceFrame* pRF = NULL;
428 OrientationDescription od
429 = ReadOptionalOrientationDescription(pDM, *this);
430 SAFENEWWITHCONSTRUCTOR(pRF,
431 ReferenceFrame,
432 ReferenceFrame(uLabel, x, R, v, w, od));
433 if (!RF.insert(RFType::value_type(uLabel, pRF)).second) {
434 silent_cerr("Reference frame " << uLabel
435 << " already defined at line " << GetLineData()
436 << std::endl);
437 throw MBDynParser::ErrReferenceAlreadyDefined(MBDYN_EXCEPT_ARGS);
438 }
439
440 if (!sName.empty()) {
441 pRF->PutName(sName);
442 }
443
444 return true;
445 }
446
447 bool
HydraulicFluid_int(void)448 MBDynParser::HydraulicFluid_int(void)
449 {
450 if (!IsArg()) {
451 silent_cerr("Parser error in MBDynParser::HydraulicFluid_int(),"
452 " arg expected at line "
453 << GetLineData() << std::endl);
454 throw HighParser::ErrColonExpected(MBDYN_EXCEPT_ARGS);
455 }
456
457 unsigned int uLabel(GetInt());
458
459 /* Nome del fluido */
460 std::string sName;
461 if (IsKeyWord("name")) {
462 const char *sTmp = GetStringWithDelims();
463 sName = sTmp;
464 }
465
466 HydraulicFluid* pHF = ReadHydraulicFluid(*this, uLabel);
467 if (pHF == NULL) {
468 silent_cerr("unable to read hydraulic fluid " << uLabel
469 << std::endl);
470 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
471 }
472
473 if (IsArg()) {
474 silent_cerr("semicolon expected after hydraulic fluid " << uLabel
475 << " (" << (sName.empty() ? "unknown" : sName.c_str()) << ") "
476 "at line " << GetLineData() << std::endl);
477 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
478 }
479
480 if (!HF.insert(HFType::value_type(uLabel, pHF)).second) {
481 silent_cerr("hydraulic fluid " << uLabel
482 << " already defined at line " << GetLineData()
483 << std::endl);
484 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
485 }
486
487 if (!sName.empty()) {
488 pHF->PutName(sName);
489 }
490
491 return true;
492 }
493
494 bool
C81Data_int(void)495 MBDynParser::C81Data_int(void)
496 {
497 if (!IsArg()) {
498 silent_cerr("Parser error in MBDynParser::C81Data_int(),"
499 " arg expected at line " << GetLineData()
500 << std::endl);
501 throw HighParser::ErrColonExpected(MBDYN_EXCEPT_ARGS);
502 }
503
504 unsigned int uLabel(GetInt());
505
506 /* Nome del profilo c81 */
507 std::string sName;
508 if (IsKeyWord("name")) {
509 const char *sTmp = GetStringWithDelims();
510 sName = sTmp;
511 }
512
513 std::string filename = GetFileName();
514 if (filename.empty()) {
515 silent_cerr("C81Data(" << uLabel << "): "
516 "invalid file at line "
517 << GetLineData() << std::endl);
518 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
519 }
520 std::ifstream in(filename.c_str());
521 if (!in) {
522 silent_cerr("C81Data(" << uLabel << "): "
523 "unable to open file '" << filename << "' at line "
524 << GetLineData() << std::endl);
525 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
526 }
527
528 DEBUGLCOUT(MYDEBUG_INPUT, "reading c81 data " << uLabel
529 << " from file '" << filename << "'" << std::endl);
530
531 C81Data* data = NULL;
532 SAFENEWWITHCONSTRUCTOR(data, C81Data, C81Data(uLabel));
533
534 doublereal dcptol = 1e-6;
535 if (IsKeyWord("tolerance")) {
536 dcptol = GetReal();
537 if (dcptol <= 0.) {
538 silent_cerr("C81Data(" << uLabel << "): "
539 "invalid c81 data tolerance at line "
540 << GetLineData() << std::endl);
541 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
542 }
543 }
544
545 bool bFF(false);
546 if (IsKeyWord("fc511")) {
547 if (c81_data_fc511_read(in, data, dcptol) != 0) {
548 silent_cerr("C81Data(" << uLabel << "): "
549 "unable to read c81 data " << uLabel
550 << " from file '" << filename << "' "
551 "in fc511 format at line " << GetLineData() << std::endl);
552 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
553 }
554
555 } else if (IsKeyWord("nrel")) {
556 if (c81_data_nrel_read(in, data, dcptol) != 0) {
557 silent_cerr("C81Data(" << uLabel << "): "
558 "unable to read c81 data " << uLabel
559 << " from file '" << filename << "' "
560 "in NREL format at line " << GetLineData() << std::endl);
561 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
562 }
563
564 } else if (IsKeyWord("free" "format")) {
565 if (c81_data_read_free_format(in, data, dcptol) != 0) {
566 silent_cerr("C81Data(" << uLabel << "): "
567 "unable to read c81 data " << uLabel
568 << " from file '" << filename << "' "
569 "in free format at line " << GetLineData() << std::endl);
570 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
571 }
572
573 bFF = true;
574
575 } else {
576 int ff = 0;
577 if (c81_data_read(in, data, dcptol, &ff) != 0) {
578 silent_cerr("C81Data(" << uLabel << "): "
579 "unable to read c81 data " << uLabel
580 << " from file '" << filename << "' "
581 "at line " << GetLineData() << std::endl);
582 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
583 }
584
585 if (ff) {
586 bFF = true;
587 }
588 }
589
590 if (IsKeyWord("flip")) {
591 (void)c81_data_flip(data);
592 }
593
594 // CL
595 if (data->al[0] > -180.) {
596 silent_cerr("C81Data(" << uLabel << "): "
597 "warning, CL alpha[0]=" << data->al[0] << " > -180 (error=" << 100*(data->al[0]/180. + 1.) << "%)" << std::endl);
598 }
599 if (data->al[data->NAL - 1] < 180.) {
600 silent_cerr("C81Data(" << uLabel << "): "
601 "warning, CL alpha[" << data->NAL - 1 << "]=" << data->al[data->NAL - 1] << " < 180 (error=" << 100*(data->al[data->NAL - 1]/180. - 1.) << "%)" << std::endl);
602 }
603 if (data->ml[0] > 0.) {
604 silent_cerr("C81Data(" << uLabel << "): "
605 "warning, CL mach[0]=" << data->ml[0] << " > 0" << std::endl);
606 }
607
608 // CD
609 if (data->ad[0] > -180.) {
610 silent_cerr("C81Data(" << uLabel << "): "
611 "warning, CD alpha[0]=" << data->ad[0] << " > -180 (error=" << 100*(data->ad[0]/180. + 1.) << "%)" << std::endl);
612 }
613 if (data->ad[data->NAD - 1] < 180.) {
614 silent_cerr("C81Data(" << uLabel << "): "
615 "warning, CD alpha[" << data->NAD - 1 << "]=" << data->ad[data->NAD - 1] << " < 180 (error=" << 100*(data->ad[data->NAD - 1]/180. - 1.) << "%)" << std::endl);
616 }
617 if (data->md[0] > 0.) {
618 silent_cerr("C81Data(" << uLabel << "): "
619 "warning, CD mach[0]=" << data->md[0] << " > 0" << std::endl);
620 }
621
622 // CM
623 if (data->am[0] > -180.) {
624 silent_cerr("C81Data(" << uLabel << "): "
625 "warning, CM alpha[0]=" << data->am[0] << " > -180 (error=" << 100*(data->am[0]/180. + 1.) << "%)" << std::endl);
626 }
627 if (data->am[data->NAM - 1] < 180.) {
628 silent_cerr("C81Data(" << uLabel << "): "
629 "warning, CM alpha[" << data->NAM - 1 << "]=" << data->am[data->NAM - 1] << " < 180 (error=" << 100*(data->am[data->NAM - 1]/180. - 1.) << "%)" << std::endl);
630 }
631 if (data->mm[0] > 0.) {
632 silent_cerr("C81Data(" << uLabel << "): "
633 "warning, CM mach[0]=" << data->mm[0] << " > 0" << std::endl);
634 }
635
636 if (IsKeyWord("echo")) {
637 const char *sOutName = GetFileName();
638 if (sOutName == NULL) {
639 silent_cerr("C81Data(" << uLabel << "): "
640 "unable to read output file name "
641 "at line " << GetLineData() << std::endl);
642 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
643 }
644
645 std::ofstream out(sOutName);
646 if (!out) {
647 silent_cerr("C81Data(" << uLabel << "): "
648 "unable to open output file "
649 "\"" << sOutName << "\" "
650 "at line " << GetLineData() << std::endl);
651 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
652 }
653
654 if (IsKeyWord("free" "format")) {
655 c81_data_write_free_format(out, data);
656
657 } else if (!IsArg()) {
658 if (bFF) {
659 c81_data_write_free_format(out, data);
660
661 } else {
662 c81_data_write(out, data);
663 }
664
665 } else {
666 silent_cerr("C81Data(" << uLabel << "): "
667 "unknown output format "
668 "at line " << GetLineData() << std::endl);
669 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
670 }
671 }
672
673 if (IsArg()) {
674 silent_cerr("semicolon expected after c81 data " << uLabel
675 << " (" << (sName.empty() ? "unknown" : sName.c_str()) << ") "
676 "at line " << GetLineData() << std::endl);
677 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
678 }
679
680 if (!AD.insert(ADType::value_type(uLabel, data)).second) {
681 silent_cerr("C81Data(" << uLabel << "): "
682 "redefined at line " << GetLineData()
683 << std::endl);
684 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
685 }
686
687 if (!sName.empty()) {
688 data->PutName(sName.c_str());
689 }
690
691 return true;
692 }
693
694 bool
ConstitutiveLaw_int(void)695 MBDynParser::ConstitutiveLaw_int(void)
696 {
697 if (!IsArg()) {
698 silent_cerr("Parser error in MBDynParser::ConstitutiveLaw_int(),"
699 " arg expected at line "
700 << GetLineData() << std::endl);
701 throw HighParser::ErrColonExpected(MBDYN_EXCEPT_ARGS);
702 }
703
704 if (pDM == 0) {
705 silent_cerr("constitutive law parsing at line "
706 << GetLineData() << " allowed "
707 "only after control data block" << std::endl);
708 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
709 }
710
711 unsigned int uLabel(GetInt());
712
713 /* Constitutive law name */
714 std::string sName;
715 if (IsKeyWord("name")) {
716 const char *sTmp = GetStringWithDelims();
717 sName = sTmp;
718 }
719
720 int dim = GetInt();
721 ConstLawType::Type clt;
722 switch (dim) {
723 case 1:
724 {
725 /* allow "reference" (copy cached constitutive law) */
726 ConstitutiveLaw1D *pCL = GetConstLaw1D(clt);
727 if (pCL == NULL) {
728 silent_cerr("unable to read constitutive law 1D "
729 << uLabel);
730 if (!sName.empty()) {
731 silent_cerr(" (" << sName << ")");
732 }
733 silent_cerr(" at line " << GetLineData()
734 << std::endl);
735 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
736 }
737
738 pCL->PutLabel(uLabel);
739 if (!sName.empty()) {
740 pCL->PutName(sName);
741 }
742
743 if (!CL1D.insert(CL1DType::value_type(uLabel, pCL)).second) {
744 silent_cerr("constitutive law 1D " << uLabel);
745 if (!sName.empty()) {
746 silent_cerr(" (" << sName << ")");
747 }
748 silent_cerr(" already defined at line "
749 << GetLineData() << std::endl);
750 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
751 }
752 break;
753 }
754
755 case 3:
756 {
757 /* allow "reference" (copy cached constitutive law) */
758 ConstitutiveLaw3D *pCL = GetConstLaw3D(clt);
759 if (pCL == NULL) {
760 silent_cerr("unable to read constitutive law 3D "
761 << uLabel);
762 if (!sName.empty()) {
763 silent_cerr(" (" << sName << ")");
764 }
765 silent_cerr(" at line " << GetLineData()
766 << std::endl);
767 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
768 }
769
770 pCL->PutLabel(uLabel);
771 if (!sName.empty()) {
772 pCL->PutName(sName);
773 }
774
775 if (!CL3D.insert(CL3DType::value_type(uLabel, pCL)).second) {
776 silent_cerr("constitutive law 3D " << uLabel);
777 if (!sName.empty()) {
778 silent_cerr(" (" << sName << ")");
779 }
780 silent_cerr(" already defined at line "
781 << GetLineData() << std::endl);
782 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
783 }
784 break;
785 }
786
787 case 6:
788 {
789 /* allow "reference" (copy cached constitutive law) */
790 ConstitutiveLaw6D *pCL = GetConstLaw6D(clt);
791 if (pCL == NULL) {
792 silent_cerr("unable to read constitutive law 6D "
793 << uLabel);
794 if (!sName.empty()) {
795 silent_cerr(" (" << sName << ")");
796 }
797 silent_cerr(" at line " << GetLineData()
798 << std::endl);
799 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
800 }
801
802 pCL->PutLabel(uLabel);
803 if (!sName.empty()) {
804 pCL->PutName(sName);
805 }
806
807 if (!CL6D.insert(CL6DType::value_type(uLabel, pCL)).second) {
808 silent_cerr("constitutive law 6D " << uLabel);
809 if (!sName.empty()) {
810 silent_cerr(" (" << sName << ")");
811 }
812 silent_cerr(" already defined at line "
813 << GetLineData() << std::endl);
814 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
815 }
816 break;
817 }
818
819 default:
820 silent_cerr("unknown constitutive law dimensionality "
821 << dim << " at line " << GetLineData()
822 << std::endl);
823 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
824 }
825
826 if (IsArg()) {
827 silent_cerr("semicolon expected after constitutive law " << uLabel
828 << " (" << (sName.empty() ? "unknown" : sName.c_str()) << ") "
829 "at line " << GetLineData() << std::endl);
830 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
831 }
832
833 return true;
834 }
835
836 const MBDynParser::RFType&
GetReferenceFrameContainer(void) const837 MBDynParser::GetReferenceFrameContainer(void) const
838 {
839 return RF;
840 }
841
842 const MBDynParser::HFType&
GetHydraulicFluidContainer(void) const843 MBDynParser::GetHydraulicFluidContainer(void) const
844 {
845 return HF;
846 }
847
GetC81DataContainer(void) const848 const MBDynParser::ADType& MBDynParser::GetC81DataContainer(void) const
849 {
850 return AD;
851 }
852
853 const MBDynParser::DCType&
GetDriveCallerContainer(void) const854 MBDynParser::GetDriveCallerContainer(void) const
855 {
856 return DC;
857 }
858
859 const MBDynParser::DC1DType&
GetDriveCaller1DContainer(void) const860 MBDynParser::GetDriveCaller1DContainer(void) const
861 {
862 return DC1D;
863 }
864
865 const MBDynParser::DC3DType&
GetDriveCaller3DContainer(void) const866 MBDynParser::GetDriveCaller3DContainer(void) const
867 {
868 return DC3D;
869 }
870
871 const MBDynParser::DC6DType&
GetDriveCaller6DContainer(void) const872 MBDynParser::GetDriveCaller6DContainer(void) const
873 {
874 return DC6D;
875 }
876
877 const MBDynParser::DC3x3DType&
GetDriveCaller3x3DContainer(void) const878 MBDynParser::GetDriveCaller3x3DContainer(void) const
879 {
880 return DC3x3D;
881 }
882
883 const MBDynParser::DC6x6DType&
GetDriveCaller6x6DContainer(void) const884 MBDynParser::GetDriveCaller6x6DContainer(void) const
885 {
886 return DC6x6D;
887 }
888
889 bool
DriveCaller_int(void)890 MBDynParser::DriveCaller_int(void)
891 {
892 if (!IsArg()) {
893 silent_cerr("Parser error in MBDynParser::DriveCaller_int(), "
894 " arg expected at line "
895 << GetLineData() << std::endl);
896 throw HighParser::ErrColonExpected(MBDYN_EXCEPT_ARGS);
897 }
898
899 unsigned int uLabel(GetInt());
900
901 /* drive name */
902 std::string sName;
903 if (IsKeyWord("name")) {
904 const char *sTmp = GetStringWithDelims();
905 sName = sTmp;
906 }
907
908 bool bDeferred(false);
909 if (IsKeyWord("deferred")) {
910 bDeferred = true;
911 }
912
913 /* allow "reference" (copy cached drive) */
914 DriveCaller *pDC = GetDriveCaller(bDeferred);
915 if (pDC == NULL) {
916 silent_cerr("unable to read drive caller " << uLabel
917 << " (" << (sName.empty() ? "unknown" : sName.c_str()) << ") "
918 "at line " << GetLineData() << std::endl);
919 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
920 }
921
922 DriveCallerRead::ReadOutput(pDC, pDM, *this);
923
924 if (IsArg()) {
925 silent_cerr("semicolon expected after drive caller " << uLabel
926 << " (" << (sName.empty() ? "unknown" : sName.c_str()) << ") "
927 "at line " << GetLineData() << std::endl);
928 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
929 }
930
931 pDC->PutLabel(uLabel);
932 if (!sName.empty()) {
933 pDC->PutName(sName);
934 }
935
936 if (!DC.insert(DCType::value_type(uLabel, pDC)).second) {
937 silent_cerr("drive caller " << uLabel);
938 if (!sName.empty()) {
939 silent_cerr(" (" << sName << ")");
940 }
941 silent_cerr(" already defined at line "
942 << GetLineData() << std::endl);
943 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
944 }
945
946 return true;
947 }
948
949 bool
TplDriveCaller_int(void)950 MBDynParser::TplDriveCaller_int(void)
951 {
952 if (!IsArg()) {
953 silent_cerr("Parser error in MBDynParser::TplDriveCaller_int(), "
954 " arg expected at line "
955 << GetLineData() << std::endl);
956 throw HighParser::ErrColonExpected(MBDYN_EXCEPT_ARGS);
957 }
958
959 unsigned int uLabel(GetInt());
960
961 /* drive name */
962 std::string sName;
963 if (IsKeyWord("name")) {
964 const char *sTmp = GetStringWithDelims();
965 sName = sTmp;
966 }
967
968 enum {
969 DIM_1,
970 DIM_3,
971 DIM_6,
972 DIM_3x3,
973 DIM_6x6
974 } Dim;
975
976 if (IsStringWithDelims()) {
977 const char *s = GetStringWithDelims();
978 if (strcasecmp(s, "1") == 0) {
979 Dim = DIM_1;
980
981 } else if (strcasecmp(s, "3") == 0) {
982 Dim = DIM_3;
983
984 } else if (strcasecmp(s, "6") == 0) {
985 Dim = DIM_6;
986
987 } else if (strcasecmp(s, "3x3") == 0) {
988 Dim = DIM_3x3;
989
990 } else if (strcasecmp(s, "6x6") == 0) {
991 Dim = DIM_6x6;
992
993 } else {
994 silent_cerr("unable to read template drive caller " << uLabel
995 << " (" << (sName.empty() ? "unknown" : sName.c_str()) << ") "
996 "unsupported type \"" << s << "\" "
997 "at line " << GetLineData() << std::endl);
998 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
999 }
1000
1001 } else {
1002 int dim = GetInt();
1003 switch (dim) {
1004 case 1:
1005 Dim = DIM_1;
1006 break;
1007
1008 case 3:
1009 Dim = DIM_3;
1010 break;
1011
1012 case 6:
1013 Dim = DIM_6;
1014 break;
1015
1016 default:
1017 silent_cerr("unable to read template drive caller " << uLabel
1018 << " (" << (sName.empty() ? "unknown" : sName.c_str()) << ") "
1019 "unsupported order " << dim << " "
1020 "at line " << GetLineData() << std::endl);
1021 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1022 }
1023 }
1024
1025 std::string type;
1026 switch (Dim) {
1027 case DIM_1: {
1028 type = "doublereal ";
1029 TplDriveCaller<doublereal> *pDC = GetTplDriveCaller<doublereal>();
1030 if (pDC == NULL) {
1031 silent_cerr("unable to read doublereal template drive caller " << uLabel
1032 << " (" << (sName.empty() ? "unknown" : sName.c_str()) << ") "
1033 "at line " << GetLineData() << std::endl);
1034 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1035 }
1036
1037 #if 0
1038 pDC->PutLabel(uLabel);
1039 if (!sName.empty()) {
1040 pDC->PutName(sName);
1041 }
1042 #endif
1043
1044 if (!DC1D.insert(DC1DType::value_type(uLabel, pDC)).second) {
1045 silent_cerr("doublereal template drive caller" << uLabel
1046 << " (" << (sName.empty() ? "unknown" : sName.c_str()) << ") "
1047 "already defined at line "
1048 << GetLineData() << std::endl);
1049 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1050 }
1051 } break;
1052
1053 case DIM_3: {
1054 type = "Vec3 ";
1055 TplDriveCaller<Vec3> *pDC = GetTplDriveCaller<Vec3>();
1056 if (pDC == NULL) {
1057 silent_cerr("unable to read Vec3 template drive caller " << uLabel
1058 << " (" << (sName.empty() ? "unknown" : sName.c_str()) << ") "
1059 "at line " << GetLineData() << std::endl);
1060 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1061 }
1062
1063 #if 0
1064 pDC->PutLabel(uLabel);
1065 if (!sName.empty()) {
1066 pDC->PutName(sName);
1067 }
1068 #endif
1069
1070 if (!DC3D.insert(DC3DType::value_type(uLabel, pDC)).second) {
1071 silent_cerr("Vec3 template drive caller" << uLabel
1072 << " (" << (sName.empty() ? "unknown" : sName.c_str()) << ") "
1073 "already defined at line "
1074 << GetLineData() << std::endl);
1075 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1076 }
1077 } break;
1078
1079 case DIM_6: {
1080 type = "Vec6 ";
1081 TplDriveCaller<Vec6> *pDC = GetTplDriveCaller<Vec6>();
1082 if (pDC == NULL) {
1083 silent_cerr("unable to read Vec6 template drive caller " << uLabel
1084 << " (" << (sName.empty() ? "unknown" : sName.c_str()) << ") "
1085 "at line " << GetLineData() << std::endl);
1086 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1087 }
1088
1089 #if 0
1090 pDC->PutLabel(uLabel);
1091 if (!sName.empty()) {
1092 pDC->PutName(sName);
1093 }
1094 #endif
1095
1096 if (!DC6D.insert(DC6DType::value_type(uLabel, pDC)).second) {
1097 silent_cerr("Vec6 template drive caller" << uLabel
1098 << " (" << (sName.empty() ? "unknown" : sName.c_str()) << ") "
1099 "already defined at line "
1100 << GetLineData() << std::endl);
1101 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1102 }
1103 } break;
1104
1105 case DIM_3x3: {
1106 type = "Mat3x3 ";
1107 TplDriveCaller<Mat3x3> *pDC = GetTplDriveCaller<Mat3x3>();
1108 if (pDC == NULL) {
1109 silent_cerr("unable to read Mat3x3 template drive caller " << uLabel
1110 << " (" << (sName.empty() ? "unknown" : sName.c_str()) << ") "
1111 "at line " << GetLineData() << std::endl);
1112 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1113 }
1114
1115 #if 0
1116 pDC->PutLabel(uLabel);
1117 if (!sName.empty()) {
1118 pDC->PutName(sName);
1119 }
1120 #endif
1121
1122 if (!DC3x3D.insert(DC3x3DType::value_type(uLabel, pDC)).second) {
1123 silent_cerr("Mat3x3 template drive caller" << uLabel
1124 << " (" << (sName.empty() ? "unknown" : sName.c_str()) << ") "
1125 "already defined at line "
1126 << GetLineData() << std::endl);
1127 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1128 }
1129 } break;
1130
1131 case DIM_6x6: {
1132 type = "Mat6x6 ";
1133 TplDriveCaller<Mat6x6> *pDC = GetTplDriveCaller<Mat6x6>();
1134 if (pDC == NULL) {
1135 silent_cerr("unable to read Mat6x6 template drive caller " << uLabel
1136 << " (" << (sName.empty() ? "unknown" : sName.c_str()) << ") "
1137 "at line " << GetLineData() << std::endl);
1138 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1139 }
1140
1141 #if 0
1142 pDC->PutLabel(uLabel);
1143 if (!sName.empty()) {
1144 pDC->PutName(sName);
1145 }
1146 #endif
1147
1148 if (!DC6x6D.insert(DC6x6DType::value_type(uLabel, pDC)).second) {
1149 silent_cerr("Mat6x6 template drive caller" << uLabel
1150 << " (" << (sName.empty() ? "unknown" : sName.c_str()) << ") "
1151 "already defined at line "
1152 << GetLineData() << std::endl);
1153 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1154 }
1155 } break;
1156 }
1157
1158 if (IsArg()) {
1159 silent_cerr("semicolon expected after " << type << "template drive caller " << uLabel
1160 << " (" << (sName.empty() ? "unknown" : sName.c_str()) << ") "
1161 "at line " << GetLineData() << std::endl);
1162 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1163 }
1164
1165 return true;
1166 }
1167
1168 bool
ScalarFunction_int(void)1169 MBDynParser::ScalarFunction_int(void)
1170 {
1171 if (!IsArg()) {
1172 silent_cerr("Parser error in MBDynParser::ScalarFunction_int(), "
1173 " arg expected at line "
1174 << GetLineData() << std::endl);
1175 throw HighParser::ErrColonExpected(MBDYN_EXCEPT_ARGS);
1176 }
1177
1178 (void)ParseScalarFunction(*this, pDM);
1179
1180 if (IsArg()) {
1181 silent_cerr("semicolon expected after scalar function "
1182 "at line " << GetLineData() << std::endl);
1183 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1184 }
1185
1186 return true;
1187 }
1188
1189 bool
ModuleLoad_int(void)1190 MBDynParser::ModuleLoad_int(void)
1191 {
1192 #ifndef USE_RUNTIME_LOADING
1193 silent_cerr("ModuleLoad_int: dynamic modules not supported; libltdl support needed"
1194 << std::endl);
1195 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1196
1197 #else // USE_RUNTIME_LOADING
1198 if (!IsArg()) {
1199 silent_cerr("Parser error in MBDynParser::ModuleLoad_int(), "
1200 " arg expected at line "
1201 << GetLineData() << std::endl);
1202 throw HighParser::ErrColonExpected(MBDYN_EXCEPT_ARGS);
1203 }
1204
1205 /* nome del modulo */
1206 const char* s = GetFileName();
1207 if (s == NULL) {
1208 silent_cerr("ModuleLoad_int: unable to get module name"
1209 << std::endl);
1210 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1211 }
1212
1213 std::string module_name(s);
1214
1215 if (!moduleInitialized) {
1216 module_initialize();
1217 moduleInitialized = true;
1218 }
1219
1220 pedantic_cout("module \"" << module_name << "\" loading..." << std::endl);
1221
1222 lt_dlhandle handle = lt_dlopenext(module_name.c_str());
1223
1224 if (handle == NULL) {
1225 const char* err = lt_dlerror();
1226 if (err == 0) {
1227 err = "";
1228 }
1229
1230 silent_cerr("ModuleLoad_int: "
1231 << "unable to open module <" << module_name
1232 << "> (" << err << ") at line " << GetLineData()
1233 << std::endl);
1234 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1235 }
1236
1237 typedef int (*sym_t)(const char *, void *, void *);
1238 sym_t sym = (sym_t)lt_dlsym(handle, "module_init");
1239 if (sym == NULL) {
1240 const char* err = lt_dlerror();
1241 if (err == NULL) {
1242 silent_cerr("ModuleLoad_int: module_init() "
1243 "function not available "
1244 "in module <" << module_name
1245 << "> at line " << GetLineData()
1246 << std::endl);
1247 } else {
1248 silent_cerr("ModuleLoad_int: "
1249 << "error while binding to symbol "
1250 "module_init() in module <" << module_name
1251 << "> (" << err << ") at line " << GetLineData()
1252 << std::endl);
1253 }
1254 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1255 }
1256
1257 if ((*sym)(module_name.c_str(), (void *)pDM, (void *)this)) {
1258 silent_cerr("ModuleLoad_int: module_init() "
1259 "of module <" << module_name
1260 << "> failed at line " << GetLineData()
1261 << std::endl);
1262 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1263 }
1264
1265 if (IsArg()) {
1266 silent_cerr("semicolon expected after module "
1267 "\"" << module_name << "\""
1268 << " at line " << GetLineData() << std::endl);
1269 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1270 }
1271
1272 silent_cout("module \"" << module_name << "\" loaded" << std::endl);
1273
1274 return true;
1275 #endif // ! USE_RUNTIME_LOADING
1276 }
1277
1278 MBDynParser::Frame
GetRef(ReferenceFrame & rf)1279 MBDynParser::GetRef(ReferenceFrame& rf)
1280 {
1281 if (!IsKeyWord("reference")) {
1282 return MBDynParser::UNKNOWNFRAME;
1283 }
1284
1285 if (IsKeyWord("global")) {
1286 return MBDynParser::GLOBAL;
1287 }
1288
1289 if (IsKeyWord("node")) {
1290 return MBDynParser::NODE;
1291 }
1292
1293 if (IsKeyWord("local")) {
1294 return MBDynParser::LOCAL;
1295 }
1296
1297 if (IsKeyWord("other" "position")) {
1298 return MBDynParser::OTHER_POSITION;
1299 }
1300
1301 if (IsKeyWord("other" "orientation")) {
1302 return MBDynParser::OTHER_ORIENTATION;
1303 }
1304
1305 if (IsKeyWord("other" "node")) {
1306 return MBDynParser::OTHER_NODE;
1307 }
1308
1309 unsigned int uLabel((unsigned int)GetInt());
1310 RFType::const_iterator i = RF.find(uLabel);
1311 if (i == RF.end()) {
1312 silent_cerr("reference " << uLabel << " is undefined at line "
1313 << GetLineData() << std::endl);
1314 throw MBDynParser::ErrReferenceUndefined(MBDYN_EXCEPT_ARGS);
1315 }
1316
1317 rf = *(i->second);
1318
1319 return MBDynParser::REFERENCE;
1320 }
1321
1322 void
OutputFrames(std::ostream & out) const1323 MBDynParser::OutputFrames(std::ostream& out) const
1324 {
1325 for (RFType::const_iterator i = RF.begin(); i != RF.end(); ++i) {
1326 i->second->Output(out);
1327 }
1328 }
1329
1330 Vec3
GetPosRel(const ReferenceFrame & rf)1331 MBDynParser::GetPosRel(const ReferenceFrame& rf)
1332 {
1333 ReferenceFrame rfOut;
1334 switch (GetRef(rfOut)) {
1335 case GLOBAL:
1336 return rf.GetR().MulTV(GetVec3() - rf.GetX());
1337
1338 case NODE:
1339 case UNKNOWNFRAME:
1340 return GetVec3();
1341
1342 case LOCAL: {
1343 Mat3x3 R(GetMatR2vec());
1344 return R*GetVec3();
1345 }
1346
1347 case REFERENCE:
1348 return rf.GetR().MulTV((rfOut.GetX() + rfOut.GetR()*GetVec3()) - rf.GetX());
1349
1350 case OTHER_POSITION:
1351 case OTHER_ORIENTATION:
1352 case OTHER_NODE:
1353 silent_cerr("GetPosRel: \"other\" meaningless in this context "
1354 "at line " << GetLineData() << std::endl);
1355 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1356
1357 default:
1358 ASSERTMSG(0, "You shouldn't have reached this point");
1359 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1360 }
1361 }
1362
1363 Vec3
GetPosRel(const ReferenceFrame & rf,const ReferenceFrame & other_rf,const Vec3 & other_X)1364 MBDynParser::GetPosRel(const ReferenceFrame& rf, const ReferenceFrame& other_rf, const Vec3& other_X)
1365 {
1366 ReferenceFrame rfOut;
1367 switch (GetRef(rfOut)) {
1368 case GLOBAL:
1369 return rf.GetR().MulTV(GetVec3() - rf.GetX());
1370
1371 case NODE:
1372 case UNKNOWNFRAME:
1373 return GetVec3();
1374
1375 case LOCAL: {
1376 Mat3x3 R(GetMatR2vec());
1377 return R*GetVec3();
1378 }
1379
1380 case REFERENCE:
1381 return rf.GetR().MulTV((rfOut.GetX() + rfOut.GetR()*GetVec3()) - rf.GetX());
1382
1383 case OTHER_POSITION:
1384 return rf.GetR().MulTV((other_rf.GetX() + other_rf.GetR()*(other_X + GetVec3())) - rf.GetX());
1385
1386 case OTHER_NODE:
1387 return rf.GetR().MulTV((other_rf.GetX() + other_rf.GetR()*GetVec3()) - rf.GetX());
1388
1389 case OTHER_ORIENTATION:
1390 silent_cerr("GetPosRel: \"other\" meaningless in this context "
1391 "at line " << GetLineData() << std::endl);
1392 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1393
1394 default:
1395 ASSERTMSG(0, "You shouldn't have reached this point");
1396 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1397 }
1398 }
1399
1400 Vec3
GetPosAbs(const ReferenceFrame & rf)1401 MBDynParser::GetPosAbs(const ReferenceFrame& rf)
1402 {
1403 ReferenceFrame rfOut;
1404 switch (GetRef(rfOut)) {
1405 case GLOBAL:
1406 case UNKNOWNFRAME:
1407 return GetVec3();
1408
1409 case NODE:
1410 return rf.GetX() + rf.GetR()*GetVec3();
1411
1412 case LOCAL: {
1413 Mat3x3 R(GetMatR2vec());
1414 return rf.GetX() + rf.GetR()*(R*GetVec3());
1415 }
1416
1417 case REFERENCE:
1418 return rfOut.GetX() + rfOut.GetR()*GetVec3();
1419
1420 case OTHER_POSITION:
1421 case OTHER_ORIENTATION:
1422 case OTHER_NODE:
1423 silent_cerr("GetPosAbs: \"other\" meaningless in this context "
1424 "at line " << GetLineData() << std::endl);
1425 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1426
1427 default:
1428 ASSERTMSG(0, "You shouldn't have reached this point");
1429 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1430 }
1431 }
1432
1433 #if 0 // TODO; not really needed: use MBDynParser::GetPosAbs(::AbsRefFrame, ...) instead
1434 Vec3
1435 MBDynParser::GetPosAbs(const ReferenceFrame& rf, const ReferenceFrame& other_rf, const Vec3& other_X)
1436 {
1437 }
1438 #endif
1439
1440 Vec3
GetVelRel(const ReferenceFrame & rf,const Vec3 & x)1441 MBDynParser::GetVelRel(const ReferenceFrame& rf, const Vec3& x)
1442 {
1443 ReferenceFrame rfOut;
1444 switch (GetRef(rfOut)) {
1445 case GLOBAL:
1446 return rf.GetR().MulTV(GetVec3()
1447 -rf.GetV()
1448 -rf.GetW().Cross(x-rf.GetX()));
1449
1450 case NODE:
1451 case UNKNOWNFRAME:
1452 return GetVec3();
1453
1454 case LOCAL: {
1455 Mat3x3 R(GetMatR2vec());
1456 return R*GetVec3();
1457 }
1458
1459 case REFERENCE:
1460 return rf.GetR().MulTV(
1461 rfOut.GetV()
1462 +rfOut.GetR()*GetVec3()
1463 +rfOut.GetW().Cross(x-rfOut.GetX())
1464 -rf.GetV()
1465 -rf.GetW().Cross(x-rf.GetX())
1466 );
1467
1468 case OTHER_POSITION:
1469 case OTHER_ORIENTATION:
1470 case OTHER_NODE:
1471 silent_cerr("GetVelRel: \"other\" meaningless in this context "
1472 "at line " << GetLineData() << std::endl);
1473 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1474
1475 default:
1476 ASSERTMSG(0, "You shouldn't have reached this point");
1477 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1478 }
1479 }
1480
1481 Vec3
GetVelAbs(const ReferenceFrame & rf,const Vec3 & x)1482 MBDynParser::GetVelAbs(const ReferenceFrame& rf, const Vec3& x)
1483 {
1484 ReferenceFrame rfOut;
1485 switch (GetRef(rfOut)) {
1486 case GLOBAL:
1487 case UNKNOWNFRAME:
1488 return GetVec3();
1489
1490 case NODE:
1491 return rf.GetV()+rf.GetR()*GetVec3()
1492 +rf.GetW().Cross(x-rf.GetX());
1493
1494 case LOCAL: {
1495 Mat3x3 R(GetMatR2vec());
1496 return rf.GetV()+rf.GetR()*(R*GetVec3())
1497 +rf.GetW().Cross(x-rf.GetX());
1498 }
1499
1500 case REFERENCE:
1501 return rfOut.GetV()+rfOut.GetR()*GetVec3()
1502 +rfOut.GetW().Cross(x-rfOut.GetX());
1503
1504 case OTHER_POSITION:
1505 case OTHER_ORIENTATION:
1506 case OTHER_NODE:
1507 silent_cerr("GetVelAbs: \"other\" meaningless in this context "
1508 "at line " << GetLineData() << std::endl);
1509 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1510
1511 default:
1512 ASSERTMSG(0, "You shouldn't have reached this point");
1513 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1514 }
1515 }
1516
1517 Vec3
GetOmeRel(const ReferenceFrame & rf)1518 MBDynParser::GetOmeRel(const ReferenceFrame& rf)
1519 {
1520 ReferenceFrame rfOut;
1521 switch (GetRef(rfOut)) {
1522 case GLOBAL:
1523 return rf.GetR().MulTV(GetVec3()-rf.GetW());
1524
1525 case NODE:
1526 case UNKNOWNFRAME:
1527 return GetVec3();
1528
1529 case LOCAL: {
1530 Mat3x3 R(GetMatR2vec());
1531 return R*GetVec3();
1532 }
1533
1534 case REFERENCE:
1535 return rf.GetR().MulTV(rfOut.GetW() + rfOut.GetR()*GetVec3() - rf.GetW());
1536
1537 case OTHER_POSITION:
1538 case OTHER_ORIENTATION:
1539 case OTHER_NODE:
1540 silent_cerr("GetOmeRel: \"other\" meaningless in this context "
1541 "at line " << GetLineData() << std::endl);
1542 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1543
1544 default:
1545 ASSERTMSG(0, "You shouldn't have reached this point");
1546 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1547 }
1548 }
1549
1550 Vec3
GetOmeAbs(const ReferenceFrame & rf)1551 MBDynParser::GetOmeAbs(const ReferenceFrame& rf)
1552 {
1553 ReferenceFrame rfOut;
1554 switch (GetRef(rfOut)) {
1555 case GLOBAL:
1556 case UNKNOWNFRAME:
1557 return GetVec3();
1558
1559 case NODE:
1560 return rf.GetW()+rf.GetR()*GetVec3();
1561
1562 case LOCAL: {
1563 Mat3x3 R(GetMatR2vec());
1564 return rf.GetW()+rf.GetR()*(R*GetVec3());
1565 }
1566
1567 case REFERENCE:
1568 return rfOut.GetW()+rfOut.GetR()*GetVec3();
1569
1570 case OTHER_POSITION:
1571 case OTHER_ORIENTATION:
1572 case OTHER_NODE:
1573 silent_cerr("GetOmeAbs: \"other\" meaningless in this context "
1574 "at line " << GetLineData() << std::endl);
1575 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1576
1577 default:
1578 ASSERTMSG(0, "You shouldn't have reached this point");
1579 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1580 }
1581 }
1582
1583 Vec3
GetVecRel(const ReferenceFrame & rf)1584 MBDynParser::GetVecRel(const ReferenceFrame& rf)
1585 {
1586 ReferenceFrame rfOut;
1587 switch (GetRef(rfOut)) {
1588 case GLOBAL:
1589 return rf.GetR().MulTV(GetVec3());
1590
1591 case UNKNOWNFRAME: /* global */
1592 if (IsKeyWord("from" "node")) {
1593 /* FIXME */
1594 silent_cerr("'from node' at line " << GetLineData()
1595 << " not implemented yet :)" << std::endl);
1596 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1597
1598 #if 0
1599 unsigned int uLabel = GetInt();
1600 StructNode *pNode1 = NULL; /* get node 1 */
1601
1602
1603 if (!IsKeyWord("to" "node")) {
1604 silent_cerr("missing keyword 'to node' at line "
1605 << GetLineData() << std::endl);
1606 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1607 }
1608
1609 uLabel = GetInt();
1610 StructNode *pNode2 = NULL; /* get node 2 */
1611
1612 Vec3 v = pNode2->GetXCurr() - pNode1->GetXCurr();
1613 return rf.GetR().MulTV(v);
1614 #endif // 0
1615 } /* else local */
1616 case NODE:
1617 return GetVec3();
1618
1619 case LOCAL: {
1620 Mat3x3 R(GetMatR2vec());
1621 return R*GetVec3();
1622 }
1623
1624 case REFERENCE:
1625 return rf.GetR().MulTV(rfOut.GetR()*GetVec3());
1626
1627 case OTHER_POSITION:
1628 case OTHER_ORIENTATION:
1629 case OTHER_NODE:
1630 silent_cerr("GetVecRel: \"other\" meaningless in this context "
1631 "at line " << GetLineData() << std::endl);
1632 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1633
1634 default:
1635 ASSERTMSG(0, "You shouldn't have reached this point");
1636 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1637 }
1638 }
1639
1640 Vec3
GetVecAbs(const ReferenceFrame & rf)1641 MBDynParser::GetVecAbs(const ReferenceFrame& rf)
1642 {
1643 ReferenceFrame rfOut;
1644 switch (GetRef(rfOut)) {
1645 case UNKNOWNFRAME: /* global */
1646 if (IsKeyWord("from" "node")) {
1647 /* FIXME */
1648 silent_cerr("'from node' at line " << GetLineData()
1649 << " not implemented yet :)" << std::endl);
1650 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1651
1652 #if 0
1653 unsigned int uLabel = GetInt();
1654 StructNode *pNode1 = NULL; /* get node 1 */
1655 if (IsKeyWord("to" "node")) {
1656 silent_cerr("missing keyword 'to node' at line "
1657 << GetLineData() << std::endl);
1658 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1659 }
1660 uLabel = GetInt();
1661 StructNode *pNode2 = NULL; /* get node 2 */
1662
1663 return pNode2->GetXCurr() - pNode1->GetXCurr();
1664 #endif // 0
1665 } /* else global: fallthru */
1666
1667 case GLOBAL:
1668 return GetVec3();
1669
1670 case NODE:
1671 return rf.GetR()*GetVec3();
1672
1673 case LOCAL: {
1674 Mat3x3 R(GetMatR2vec());
1675 return rf.GetR()*(R*GetVec3());
1676 }
1677
1678 case REFERENCE:
1679 return rfOut.GetR()*GetVec3();
1680
1681 case OTHER_POSITION:
1682 case OTHER_ORIENTATION:
1683 case OTHER_NODE:
1684 silent_cerr("GetVecAbs: \"other\" meaningless in this context "
1685 "at line " << GetLineData() << std::endl);
1686 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1687
1688 default:
1689 ASSERTMSG(0, "You shouldn't have reached this point");
1690 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1691 }
1692 }
1693
1694 Vec3
GetUnitVecRel(const ReferenceFrame & rf)1695 MBDynParser::GetUnitVecRel(const ReferenceFrame& rf)
1696 {
1697 Vec3 v = GetVecRel(rf);
1698
1699 doublereal d = v.Dot();
1700 if (d <= std::numeric_limits<doublereal>::epsilon()) {
1701 throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
1702 }
1703
1704 d = std::sqrt(d);
1705
1706 if (std::fabs(d - 1.) > std::numeric_limits<doublereal>::epsilon()) {
1707 silent_cerr("warning: non-unit vector (norm=" << d << ") "
1708 "at line " << GetLineData() << "; "
1709 "normalized" << std::endl);
1710 }
1711
1712 return v /= d;
1713 }
1714
1715 Vec3
GetUnitVecAbs(const ReferenceFrame & rf)1716 MBDynParser::GetUnitVecAbs(const ReferenceFrame& rf)
1717 {
1718 Vec3 v = GetVecAbs(rf);
1719
1720 doublereal d = v.Dot();
1721 if (d <= std::numeric_limits<doublereal>::epsilon()) {
1722 throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
1723 }
1724
1725 d = std::sqrt(d);
1726
1727 if (std::fabs(d - 1.) > std::numeric_limits<doublereal>::epsilon()) {
1728 silent_cerr("warning: non-unit vector (norm=" << d << ") "
1729 "at line " << GetLineData() << "; "
1730 "normalized" << std::endl);
1731 }
1732
1733 return v /= d;
1734 }
1735
1736 Mat3x3
GetMatRel(const ReferenceFrame & rf)1737 MBDynParser::GetMatRel(const ReferenceFrame& rf)
1738 {
1739 ReferenceFrame rfOut;
1740 switch (GetRef(rfOut)) {
1741 case GLOBAL:
1742 return rf.GetR().MulTM(GetMat3x3()*rf.GetR());
1743
1744 case NODE:
1745 case LOCAL:
1746 case UNKNOWNFRAME:
1747 return GetMat3x3();
1748
1749 case REFERENCE:
1750 return rf.GetR().MulTM(rfOut.GetR()*GetMat3x3()*rfOut.GetR().MulTM(rf.GetR()));
1751
1752 case OTHER_POSITION:
1753 case OTHER_ORIENTATION:
1754 case OTHER_NODE:
1755 silent_cerr("GetMatRel: \"other\" meaningless in this context "
1756 "at line " << GetLineData() << std::endl);
1757 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1758
1759 default:
1760 ASSERTMSG(0, "You shouldn't have reached this point");
1761 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1762 }
1763 }
1764
1765 Mat3x3
GetMatAbs(const ReferenceFrame & rf)1766 MBDynParser::GetMatAbs(const ReferenceFrame& rf)
1767 {
1768 ReferenceFrame rfOut;
1769 switch (GetRef(rfOut)) {
1770 case GLOBAL:
1771 case UNKNOWNFRAME:
1772 return GetMat3x3();
1773
1774 case NODE:
1775 case LOCAL:
1776 return rf.GetR()*(GetMat3x3()*rf.GetR().Transpose());
1777
1778 case REFERENCE:
1779 return rfOut.GetR()*(GetMat3x3()*rfOut.GetR().Transpose());
1780
1781 case OTHER_POSITION:
1782 case OTHER_ORIENTATION:
1783 case OTHER_NODE:
1784 silent_cerr("GetMatAbs: \"other\" meaningless in this context "
1785 "at line " << GetLineData() << std::endl);
1786 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1787
1788 default:
1789 ASSERTMSG(0, "You shouldn't have reached this point");
1790 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1791 }
1792 }
1793
1794 Mat3x3
GetRotRel(const ReferenceFrame & rf)1795 MBDynParser::GetRotRel(const ReferenceFrame& rf)
1796 {
1797 ReferenceFrame rfOut;
1798 switch (GetRef(rfOut)) {
1799 case GLOBAL:
1800 return rf.GetR().MulTM(GetMatR2vec());
1801
1802 case NODE:
1803 case LOCAL:
1804 case UNKNOWNFRAME:
1805 return GetMatR2vec();
1806
1807 case REFERENCE:
1808 return rf.GetR().MulTM(rfOut.GetR()*GetMatR2vec());
1809
1810 case OTHER_POSITION:
1811 case OTHER_ORIENTATION:
1812 case OTHER_NODE:
1813 silent_cerr("GetRotRel: \"other\" meaningless in this context "
1814 "at line " << GetLineData() << std::endl);
1815 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1816
1817 default:
1818 ASSERTMSG(0, "You shouldn't have reached this point");
1819 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1820 }
1821 }
1822
1823 Mat3x3
GetRotRel(const ReferenceFrame & rf,const ReferenceFrame & other_rf,const Mat3x3 & other_R)1824 MBDynParser::GetRotRel(const ReferenceFrame& rf, const ReferenceFrame& other_rf, const Mat3x3& other_R)
1825 {
1826 ReferenceFrame rfOut;
1827 switch (GetRef(rfOut)) {
1828 case GLOBAL:
1829 return rf.GetR().MulTM(GetMatR2vec());
1830
1831 case NODE:
1832 case LOCAL:
1833 case UNKNOWNFRAME:
1834 return GetMatR2vec();
1835
1836 case REFERENCE:
1837 return rf.GetR().MulTM(rfOut.GetR()*GetMatR2vec());
1838
1839 case OTHER_POSITION:
1840 silent_cerr("GetRotRel: \"other\" meaningless in this context "
1841 "at line " << GetLineData() << std::endl);
1842 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1843
1844 case OTHER_ORIENTATION:
1845 return rf.GetR().MulTM(other_rf.GetR()*other_R*GetMatR2vec());
1846
1847 case OTHER_NODE:
1848 return rf.GetR().MulTM(other_rf.GetR()*GetMatR2vec());
1849
1850 default:
1851 ASSERTMSG(0, "You shouldn't have reached this point");
1852 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1853 }
1854 }
1855
1856 Mat3x3
GetRotAbs(const ReferenceFrame & rf)1857 MBDynParser::GetRotAbs(const ReferenceFrame& rf)
1858 {
1859 ReferenceFrame rfOut;
1860 switch (GetRef(rfOut)) {
1861 case GLOBAL:
1862 case UNKNOWNFRAME:
1863 return GetMatR2vec();
1864
1865 case NODE:
1866 case LOCAL:
1867 return rf.GetR()*GetMatR2vec();
1868
1869 case REFERENCE:
1870 return rfOut.GetR()*GetMatR2vec();
1871
1872 case OTHER_POSITION:
1873 case OTHER_ORIENTATION:
1874 case OTHER_NODE:
1875 silent_cerr("GetRotAbs: \"other\" meaningless in this context "
1876 "at line " << GetLineData() << std::endl);
1877 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1878
1879 default:
1880 ASSERTMSG(0, "You shouldn't have reached this point");
1881 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1882 }
1883 }
1884
1885 #if 0 // TODO; not really needed: use MBDynParser::GetRotAbs(::AbsRefFrame, ...) instead
1886 Mat3x3
1887 MBDynParser::GetRotAbs(const ReferenceFrame& rf, const ReferenceFrame& other_rf, const Mat3x3& other_R)
1888 {
1889 }
1890 #endif
1891
1892 HydraulicFluid*
GetHydraulicFluid(void)1893 MBDynParser::GetHydraulicFluid(void)
1894 {
1895 /* verifica che sia stato chiamato con "hydraulic" "fluid" */
1896 if (!IsKeyWord("hydraulic" "fluid") && !IsKeyWord("fluid")) {
1897 silent_cerr("hydraulic fluid expected at line "
1898 << GetLineData() << std::endl);
1899 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1900 }
1901
1902 /* se non trova "reference", legge direttamente un fluido */
1903 if (!IsKeyWord("reference")) {
1904 return ReadHydraulicFluid(*this, 0);
1905 }
1906
1907 /* altrimenti usa un fluido predefinito, se lo trova */
1908 unsigned int uLabel = GetInt();
1909 HFType::const_iterator i = HF.find(uLabel);
1910 if (i == HF.end()) {
1911 silent_cerr("hydraulic fluid " << uLabel
1912 << " is undefined at line " << GetLineData()
1913 << std::endl);
1914 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1915 }
1916
1917 return i->second->pCopy();
1918 }
1919
1920 const c81_data*
GetC81Data(unsigned profile) const1921 MBDynParser::GetC81Data(unsigned profile) const
1922 {
1923 /* cerca i dati predefiniti, se li trova */
1924 ADType::const_iterator i = AD.find(profile);
1925 if (i == AD.end()) {
1926 silent_cerr("c81 data " << profile << " is undefined at line "
1927 << GetLineData() << std::endl);
1928 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1929 }
1930
1931 if (i == AD.end()) {
1932 silent_cerr("c81 data " << profile << " is undefined at line "
1933 << GetLineData() << std::endl);
1934 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1935 }
1936
1937 return i->second;
1938 }
1939
1940 ConstitutiveLaw1D *
GetConstLaw1D(ConstLawType::Type & clt)1941 MBDynParser::GetConstLaw1D(ConstLawType::Type& clt)
1942 {
1943 if (pDM == 0) {
1944 silent_cerr("constitutive law parsing at line "
1945 << GetLineData() << " allowed "
1946 "only after control data block" << std::endl);
1947 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1948 }
1949
1950 if (!IsKeyWord("reference")) {
1951 return ReadCL1D(pDM, *this, clt);
1952 }
1953
1954 unsigned int uLabel = GetInt();
1955 CL1DType::const_iterator i = CL1D.find(uLabel);
1956 if (i == CL1D.end()) {
1957 silent_cerr("constitutive law 1D " << uLabel
1958 << " is undefined at line "
1959 << GetLineData() << std::endl);
1960 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1961 }
1962
1963 clt = i->second->GetConstLawType();
1964 return i->second->pCopy();
1965 }
1966
1967 ConstitutiveLaw3D *
GetConstLaw3D(ConstLawType::Type & clt)1968 MBDynParser::GetConstLaw3D(ConstLawType::Type& clt)
1969 {
1970 if (pDM == 0) {
1971 silent_cerr("constitutive law parsing at line "
1972 << GetLineData() << " allowed "
1973 "only after control data block" << std::endl);
1974 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1975 }
1976
1977 if (!IsKeyWord("reference")) {
1978 return ReadCL3D(pDM, *this, clt);
1979 }
1980
1981 unsigned int uLabel = GetInt();
1982 CL3DType::const_iterator i = CL3D.find(uLabel);
1983 if (i == CL3D.end()) {
1984 silent_cerr("constitutive law 3D " << uLabel
1985 << " is undefined at line "
1986 << GetLineData() << std::endl);
1987 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
1988 }
1989
1990 clt = i->second->GetConstLawType();
1991 return i->second->pCopy();
1992 }
1993
1994 ConstitutiveLaw6D *
GetConstLaw6D(ConstLawType::Type & clt)1995 MBDynParser::GetConstLaw6D(ConstLawType::Type& clt)
1996 {
1997 if (pDM == 0) {
1998 silent_cerr("constitutive law parsing at line "
1999 << GetLineData() << " allowed "
2000 "only after control data block" << std::endl);
2001 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
2002 }
2003
2004 if (!IsKeyWord("reference")) {
2005 return ReadCL6D(pDM, *this, clt);
2006 }
2007
2008 unsigned int uLabel = GetInt();
2009 CL6DType::const_iterator i = CL6D.find(uLabel);
2010 if (i == CL6D.end()) {
2011 silent_cerr("constitutive law 6D " << uLabel
2012 << " is undefined at line "
2013 << GetLineData() << std::endl);
2014 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
2015 }
2016
2017 clt = i->second->GetConstLawType();
2018 return i->second->pCopy();
2019 }
2020
2021 const DriveCaller *
GetDrive(unsigned uLabel) const2022 MBDynParser::GetDrive(unsigned uLabel) const
2023 {
2024 DCType::const_iterator i = DC.find(uLabel);
2025 if (i == DC.end()) {
2026 return 0;
2027 }
2028
2029 return i->second;
2030 }
2031
2032 DriveCaller *
GetDriveCaller(bool bDeferred)2033 MBDynParser::GetDriveCaller(bool bDeferred)
2034 {
2035 if (!IsKeyWord("reference")) {
2036 DriveCaller *pDC = 0;
2037 try {
2038 pDC = ReadDriveCallerData(pDM, *this, bDeferred);
2039 }
2040 catch (DataManager::ErrNeedDataManager) {
2041 silent_cerr("the required drive caller must appear "
2042 "inside or after the \"control data\" "
2043 "block"
2044 << std::endl);
2045 throw DataManager::ErrNeedDataManager(MBDYN_EXCEPT_ARGS);
2046 }
2047 return pDC;
2048 }
2049
2050 unsigned int uLabel = GetInt();
2051 const DriveCaller *pDC = GetDrive(uLabel);
2052 if (pDC == 0) {
2053 silent_cerr("drive caller " << uLabel
2054 << " is undefined at line "
2055 << GetLineData() << std::endl);
2056 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
2057 }
2058
2059 return pDC->pCopy();
2060 }
2061
2062 template <class T>
2063 const TplDriveCaller<T> *
GetTplDrive(unsigned uLabel) const2064 MBDynParser::GetTplDrive(unsigned uLabel) const
2065 {
2066 if (typeid(T) == typeid(doublereal)) {
2067 DC1DType::const_iterator i = DC1D.find(uLabel);
2068 if (i == DC1D.end()) {
2069 return 0;
2070 }
2071
2072 return dynamic_cast<const TplDriveCaller<T> *>(i->second);
2073
2074 } else if (typeid(T) == typeid(Vec3)) {
2075 DC3DType::const_iterator i = DC3D.find(uLabel);
2076 if (i == DC3D.end()) {
2077 return 0;
2078 }
2079
2080 return dynamic_cast<const TplDriveCaller<T> *>(i->second);
2081
2082 } else if (typeid(T) == typeid(Vec6)) {
2083 DC6DType::const_iterator i = DC6D.find(uLabel);
2084 if (i == DC6D.end()) {
2085 return 0;
2086 }
2087
2088 return dynamic_cast<const TplDriveCaller<T> *>(i->second);
2089
2090 } else if (typeid(T) == typeid(Mat3x3)) {
2091 DC3x3DType::const_iterator i = DC3x3D.find(uLabel);
2092 if (i == DC3x3D.end()) {
2093 return 0;
2094 }
2095
2096 return dynamic_cast<const TplDriveCaller<T> *>(i->second);
2097
2098 } else if (typeid(T) == typeid(Mat6x6)) {
2099 DC6x6DType::const_iterator i = DC6x6D.find(uLabel);
2100 if (i == DC6x6D.end()) {
2101 return 0;
2102 }
2103
2104 return dynamic_cast<const TplDriveCaller<T> *>(i->second);
2105 }
2106
2107 return 0;
2108 }
2109
2110 template <class T>
2111 TplDriveCaller<T> *
GetTplDriveCaller(void)2112 MBDynParser::GetTplDriveCaller(void)
2113 {
2114 if (!IsKeyWord("reference")) {
2115 TplDriveCaller<T> *pDC = 0;
2116 try {
2117 if (typeid(T) == typeid(doublereal)) {
2118 pDC = dynamic_cast<TplDriveCaller<T> *>(ReadDC1D(pDM, *this));
2119
2120 } else if (typeid(T) == typeid(Vec3)) {
2121 pDC = dynamic_cast<TplDriveCaller<T> *>(ReadDC3D(pDM, *this));
2122
2123 } else if (typeid(T) == typeid(Vec6)) {
2124 pDC = dynamic_cast<TplDriveCaller<T> *>(ReadDC6D(pDM, *this));
2125
2126 } else if (typeid(T) == typeid(Mat3x3)) {
2127 pDC = dynamic_cast<TplDriveCaller<T> *>(ReadDC3x3D(pDM, *this));
2128
2129 } else if (typeid(T) == typeid(Mat6x6)) {
2130 pDC = dynamic_cast<TplDriveCaller<T> *>(ReadDC6x6D(pDM, *this));
2131
2132 } else {
2133 throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
2134 }
2135 }
2136 catch (DataManager::ErrNeedDataManager) {
2137 silent_cerr("the required drive caller must appear "
2138 "inside or after the \"control data\" block"
2139 << std::endl);
2140 throw DataManager::ErrNeedDataManager(MBDYN_EXCEPT_ARGS);
2141 }
2142 return pDC;
2143 }
2144
2145 unsigned int uLabel = GetInt();
2146 const TplDriveCaller<T> *pDC = GetTplDrive<T>(uLabel);
2147 if (pDC == 0) {
2148 silent_cerr("template drive caller " << uLabel
2149 << " is undefined at line "
2150 << GetLineData() << std::endl);
2151 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
2152 }
2153
2154 return pDC->pCopy();
2155 }
2156
2157 const BasicScalarFunction *
GetScalarFunction(void)2158 MBDynParser::GetScalarFunction(void)
2159 {
2160 if (!IsKeyWord("reference")) {
2161 return ParseScalarFunction(*this, pDM);
2162 }
2163
2164 std::string s(GetStringWithDelims());
2165 return GetScalarFunction(s);
2166 }
2167
2168 const BasicScalarFunction *
GetScalarFunction(const std::string & s)2169 MBDynParser::GetScalarFunction(const std::string &s)
2170 {
2171 SFType::const_iterator i = SF.find(s);
2172 if (i != SF.end()) {
2173 return i->second;
2174 }
2175 return 0;
2176 }
2177
2178 bool
SetScalarFunction(const std::string & s,const BasicScalarFunction * sf)2179 MBDynParser::SetScalarFunction(const std::string &s, const BasicScalarFunction *sf)
2180 {
2181 return SF.insert(SFType::value_type(s, sf)).second;
2182 }
2183
2184 void
PopManip(void)2185 MBDynParser::PopManip(void)
2186 {
2187 manip.pop();
2188 }
2189
2190 void
PushManip(const Manip * m)2191 MBDynParser::PushManip(const Manip *m)
2192 {
2193 manip.push(m);
2194 }
2195
2196 const MBDynParser::Manip *
GetManip(void) const2197 MBDynParser::GetManip(void) const
2198 {
2199 if (manip.empty()) {
2200 return 0;
2201 }
2202
2203 return manip.top();
2204 }
2205
2206 bool
bEmptyManip(void) const2207 MBDynParser::bEmptyManip(void) const
2208 {
2209 return manip.empty();
2210 }
2211
2212 doublereal
Get(const doublereal & d)2213 MBDynParser::Get(const doublereal& d)
2214 {
2215 return GetReal(doublereal(d));
2216 }
2217
2218 /* Legge un Vec3 */
2219 Vec3
GetVec3(void)2220 MBDynParser::GetVec3(void)
2221 {
2222 Vec3 v(Zero3);
2223 return GetVec3(v);
2224 }
2225
2226
2227 /* Legge un Vec3 */
2228 Vec3
GetVec3(const Vec3 & vDef)2229 MBDynParser::GetVec3(const Vec3& vDef)
2230 {
2231 if (IsKeyWord("default")) {
2232 return vDef;
2233 }
2234
2235 if (IsKeyWord("null")) {
2236 return Zero3;
2237 }
2238
2239 doublereal x1 = GetReal(vDef(1));
2240 doublereal x2 = GetReal(vDef(2));
2241 doublereal x3 = GetReal(vDef(3));
2242
2243 Vec3 v(x1, x2, x3);
2244
2245 if (IsKeyWord("scale")) {
2246 v *= GetReal(1.);
2247 }
2248
2249 return v;
2250 }
2251
2252
2253 /* Legge una matrice di orientazione sotto forma di due vettori (oppure eye) */
2254 Mat3x3
GetMatR2vec(void)2255 MBDynParser::GetMatR2vec(void)
2256 {
2257 if (IsKeyWord("eye")) {
2258 return Eye3;
2259 }
2260
2261 if (IsKeyWord("matr")) {
2262 doublereal r11 = GetReal();
2263 doublereal r12 = GetReal();
2264 doublereal r13 = GetReal();
2265 doublereal r21 = GetReal();
2266 doublereal r22 = GetReal();
2267 doublereal r23 = GetReal();
2268 doublereal r31 = GetReal();
2269 doublereal r32 = GetReal();
2270 doublereal r33 = GetReal();
2271
2272 Mat3x3 R(r11, r21, r31, r12, r22, r32, r13, r23, r33);
2273 doublereal dTol = 1.e-12;
2274 if (IsKeyWord("tolerance")) {
2275 dTol = GetReal();
2276 if (dTol < 0.) {
2277 silent_cerr("invalid (negative) tolerance while reading orientation matrix in \"matr\" format at line " << GetLineData() << std::endl);
2278 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
2279 }
2280 }
2281
2282 if (!Eye3.IsSame(R.MulTM(R), dTol)) {
2283 silent_cerr("warning: orientation matrix orthogonality not within tolerance at line " << GetLineData() << std::endl);
2284 // not an error?
2285 // throw ErrGeneric(MBDYN_EXCEPT_ARGS);
2286 }
2287
2288 return R;
2289 }
2290
2291 if (IsKeyWord("euler" "parameters")) {
2292 #if 0 /* FIXME: this function is TODO */
2293 doublereal e0 = GetReal();
2294 doublereal e1 = GetReal();
2295 doublereal e2 = GetReal();
2296 doublereal e3 = GetReal();
2297
2298 return EulerParams2MatR(Vec3(e1, e2, e3));
2299 #else
2300 silent_cerr("Line " << GetLineData()
2301 << ": Euler parameters not implemented yet"
2302 << std::endl);
2303 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
2304 #endif
2305 }
2306
2307 if (IsKeyWord("euler") || IsKeyWord("euler" "123")) {
2308 doublereal dScale = 1.;
2309 if (IsKeyWord("degrees")) {
2310 dScale = M_PI/180;
2311 }
2312
2313 doublereal e1 = GetReal();
2314 doublereal e2 = GetReal();
2315 doublereal e3 = GetReal();
2316
2317 return EulerAngles123_2MatR(Vec3(e1, e2, e3)*dScale);
2318 }
2319
2320 if (IsKeyWord("euler" "313")) {
2321 doublereal dScale = 1.;
2322 if (IsKeyWord("degrees")) {
2323 dScale = M_PI/180;
2324 }
2325
2326 doublereal e1 = GetReal();
2327 doublereal e2 = GetReal();
2328 doublereal e3 = GetReal();
2329
2330 return EulerAngles313_2MatR(Vec3(e1, e2, e3)*dScale);
2331 }
2332
2333 if (IsKeyWord("euler" "321")) {
2334 doublereal dScale = 1.;
2335 if (IsKeyWord("degrees")) {
2336 dScale = M_PI/180;
2337 }
2338
2339 doublereal e1 = GetReal();
2340 doublereal e2 = GetReal();
2341 doublereal e3 = GetReal();
2342
2343 return EulerAngles321_2MatR(Vec3(e1, e2, e3)*dScale);
2344 }
2345
2346 if (IsKeyWord("vector")) {
2347 doublereal phi1 = GetReal();
2348 doublereal phi2 = GetReal();
2349 doublereal phi3 = GetReal();
2350
2351 return RotManip::Rot(Vec3(phi1, phi2, phi3));
2352 }
2353
2354 int i1 = GetInt();
2355 Vec3 v1;
2356 v1(1) = GetReal();
2357 v1(2) = GetReal();
2358 v1(3) = GetReal();
2359
2360 int i2 = GetInt();
2361 Vec3 v2(Zero3);
2362
2363 if (IsKeyWord("guess")) {
2364 int i_max = 1;
2365 int i_min = 1;
2366
2367 if (fabs(v1(2)) > fabs(v1(1))) {
2368 i_max = 2;
2369 } else {
2370 i_min = 2;
2371 }
2372
2373 if (fabs(v1(3)) > fabs(v1(i_max))) {
2374 i_max = 3;
2375 } else if (fabs(v1(3)) < fabs(v1(i_min))) {
2376 i_min = 3;
2377 }
2378
2379 v2(i_min) = 1.;
2380 v2(i_max) = -v1(i_min)/v1(i_max);
2381
2382 } else {
2383 v2(1) = GetReal();
2384 v2(2) = GetReal();
2385 v2(3) = GetReal();
2386 }
2387
2388 Mat3x3 R;
2389 try {
2390 R = MatR2vec(i1, v1, i2, v2);
2391 } catch (std::exception& e) {
2392 silent_cerr("MBDynParser::GetMatR2vec: " << e.what() << " at line " << GetLineData() << std::endl);
2393 throw e;
2394 }
2395
2396 return R;
2397 }
2398
2399
2400 /* Legge una matrice 3x3 simmetrica come diagonale o triangolare superiore */
2401 Mat3x3
GetMat3x3Sym(void)2402 MBDynParser::GetMat3x3Sym(void)
2403 {
2404 if (IsKeyWord("null")) {
2405 return Zero3x3;
2406 }
2407
2408 Mat3x3 m;
2409
2410 if (IsKeyWord("eye")) {
2411 m = Eye3;
2412
2413 } else if (IsKeyWord("diag")) {
2414 doublereal m11 = GetReal();
2415 doublereal m22 = GetReal();
2416 doublereal m33 = GetReal();
2417 m = Mat3x3(m11, 0., 0., 0., m22, 0., 0., 0., m33);
2418
2419 } else {
2420 doublereal m11 = GetReal();
2421 doublereal m12 = GetReal();
2422 doublereal m13 = GetReal();
2423 doublereal m22 = GetReal();
2424 doublereal m23 = GetReal();
2425 doublereal m33 = GetReal();
2426 m = Mat3x3(m11, m12, m13, m12, m22, m23, m13, m23, m33);
2427 }
2428
2429 if (IsKeyWord("scale")) {
2430 m *= GetReal(1.);
2431 }
2432
2433 return m;
2434 }
2435
2436 /* Legge una matrice 3x3 generica (diagonale o nulla) */
2437 Mat3x3
GetMat3x3(void)2438 MBDynParser::GetMat3x3(void)
2439 {
2440 Mat3x3 m(Zero3x3);
2441 return GetMat3x3(m);
2442 }
2443
2444
2445 /* Legge una matrice 3x3 generica (diagonale o nulla) */
2446 Mat3x3
GetMat3x3(const Mat3x3 & mDef)2447 MBDynParser::GetMat3x3(const Mat3x3& mDef)
2448 {
2449 if (IsKeyWord("default")) {
2450 return mDef;
2451
2452 } else if (IsKeyWord("null")) {
2453 return Zero3x3;
2454 }
2455
2456 Mat3x3 m;
2457
2458 if (IsKeyWord("eye")) {
2459 m = Eye3;
2460
2461 } else if (IsKeyWord("diag")) {
2462 doublereal m11 = GetReal(mDef(1, 1));
2463 doublereal m22 = GetReal(mDef(2, 2));
2464 doublereal m33 = GetReal(mDef(3, 3));
2465 m = Mat3x3(m11, 0., 0., 0., m22, 0., 0., 0., m33);
2466
2467 } else if (IsKeyWord("sym")) {
2468 doublereal m11 = GetReal(mDef(1, 1));
2469 doublereal m12 = GetReal(mDef(1, 2));
2470 doublereal m13 = GetReal(mDef(1, 3));
2471 doublereal m22 = GetReal(mDef(2, 2));
2472 doublereal m23 = GetReal(mDef(2, 3));
2473 doublereal m33 = GetReal(mDef(3, 3));
2474 m = Mat3x3(m11, m12, m13, m12, m22, m23, m13, m23, m33);
2475
2476 } else if (IsKeyWord("skew")) {
2477 doublereal v1 = GetReal(mDef(3, 2));
2478 doublereal v2 = GetReal(mDef(1, 3));
2479 doublereal v3 = GetReal(mDef(2, 1));
2480 m = Mat3x3(0., -v3, v2, v3, 0., -v1, -v2, v1, 0.);
2481
2482 } else {
2483 if (IsKeyWord("matr")) {
2484 /* eat it; not required */
2485 NO_OP;
2486 }
2487
2488 doublereal m11 = GetReal(mDef(1, 1));
2489 doublereal m12 = GetReal(mDef(1, 2));
2490 doublereal m13 = GetReal(mDef(1, 3));
2491 doublereal m21 = GetReal(mDef(2, 1));
2492 doublereal m22 = GetReal(mDef(2, 2));
2493 doublereal m23 = GetReal(mDef(2, 3));
2494 doublereal m31 = GetReal(mDef(3, 1));
2495 doublereal m32 = GetReal(mDef(3, 2));
2496 doublereal m33 = GetReal(mDef(3, 3));
2497 m = Mat3x3(m11, m21, m31, m12, m22, m32, m13, m23, m33);
2498 }
2499
2500 if (IsKeyWord("scale")) {
2501 m *= GetReal(1.);
2502 }
2503
2504 return m;
2505 }
2506
2507
2508 /* Legge un Vec6 */
2509 Vec6
GetVec6(void)2510 MBDynParser::GetVec6(void)
2511 {
2512 Vec6 v(Zero6);
2513 return GetVec6(v);
2514 }
2515
2516
2517 /* Legge un Vec6 */
2518 Vec6
GetVec6(const Vec6 & vDef)2519 MBDynParser::GetVec6(const Vec6& vDef)
2520 {
2521 if (IsKeyWord("null")) {
2522 return Zero6;
2523 }
2524
2525 if (IsKeyWord("default")) {
2526 return vDef;
2527 }
2528
2529 doublereal x1 = GetReal(vDef(1));
2530 doublereal x2 = GetReal(vDef(2));
2531 doublereal x3 = GetReal(vDef(3));
2532 doublereal x4 = GetReal(vDef(4));
2533 doublereal x5 = GetReal(vDef(5));
2534 doublereal x6 = GetReal(vDef(6));
2535 Vec6 v(x1, x2, x3, x4, x5, x6);
2536
2537 if (IsKeyWord("scale")) {
2538 v *= GetReal(1.);
2539 }
2540
2541 return v;
2542 }
2543
2544
2545 /* Legge una matrice 6x6 generica (diagonale o nulla) */
2546 Mat6x6
GetMat6x6(void)2547 MBDynParser::GetMat6x6(void)
2548 {
2549 Mat6x6 m(Zero6x6);
2550 return GetMat6x6(m);
2551 }
2552
2553
2554 /* Legge una matrice 6x6 generica (diagonale o nulla) */
2555 Mat6x6
GetMat6x6(const Mat6x6 & mDef)2556 MBDynParser::GetMat6x6(const Mat6x6& mDef)
2557 {
2558 if (IsKeyWord("null")) {
2559 return Zero6x6;
2560
2561 } else if (IsKeyWord("default")) {
2562 return mDef;
2563 }
2564
2565 Mat6x6 m;
2566
2567 if (IsKeyWord("eye")) {
2568 m = Eye6;
2569
2570 } else if (IsKeyWord("diag")) {
2571 doublereal m11 = GetReal(mDef(1, 1));
2572 doublereal m22 = GetReal(mDef(2, 2));
2573 doublereal m33 = GetReal(mDef(3, 3));
2574 doublereal m44 = GetReal(mDef(4, 4));
2575 doublereal m55 = GetReal(mDef(5, 5));
2576 doublereal m66 = GetReal(mDef(6, 6));
2577 m = Mat6x6(
2578 m11, 0., 0., 0., 0., 0.,
2579 0., m22, 0., 0., 0., 0.,
2580 0., 0., m33, 0., 0., 0.,
2581 0., 0., 0., m44, 0., 0.,
2582 0., 0., 0., 0., m55, 0.,
2583 0., 0., 0., 0., 0., m66);
2584
2585 } else if (IsKeyWord("sym")) {
2586 doublereal m11 = GetReal(mDef(1, 1));
2587 doublereal m12 = GetReal(mDef(1, 2));
2588 doublereal m13 = GetReal(mDef(1, 3));
2589 doublereal m14 = GetReal(mDef(1, 4));
2590 doublereal m15 = GetReal(mDef(1, 5));
2591 doublereal m16 = GetReal(mDef(1, 6));
2592
2593 doublereal m22 = GetReal(mDef(2, 2));
2594 doublereal m23 = GetReal(mDef(2, 3));
2595 doublereal m24 = GetReal(mDef(2, 4));
2596 doublereal m25 = GetReal(mDef(2, 5));
2597 doublereal m26 = GetReal(mDef(2, 6));
2598
2599 doublereal m33 = GetReal(mDef(3, 3));
2600 doublereal m34 = GetReal(mDef(3, 4));
2601 doublereal m35 = GetReal(mDef(3, 5));
2602 doublereal m36 = GetReal(mDef(3, 6));
2603
2604 doublereal m44 = GetReal(mDef(4, 4));
2605 doublereal m45 = GetReal(mDef(4, 5));
2606 doublereal m46 = GetReal(mDef(4, 6));
2607
2608 doublereal m55 = GetReal(mDef(5, 5));
2609 doublereal m56 = GetReal(mDef(5, 6));
2610
2611 doublereal m66 = GetReal(mDef(6, 6));
2612
2613 m = Mat6x6(
2614 m11, m12, m13, m14, m15, m16,
2615 m12, m22, m23, m24, m25, m26,
2616 m13, m23, m33, m34, m35, m36,
2617 m14, m24, m34, m44, m45, m46,
2618 m15, m25, m35, m45, m55, m56,
2619 m16, m26, m36, m46, m56, m66);
2620
2621 } else if (IsKeyWord("anba")) {
2622 /* Formato ANBA, in cui vale la trasformazione:
2623 * ex = e2
2624 * ey = e3
2625 * ez = e1
2626 */
2627 doublereal m22 = GetReal(mDef(2, 2));
2628 doublereal m23 = GetReal(mDef(2, 3));
2629 doublereal m21 = GetReal(mDef(2, 1));
2630 doublereal m25 = GetReal(mDef(2, 5));
2631 doublereal m26 = GetReal(mDef(2, 6));
2632 doublereal m24 = GetReal(mDef(2, 4));
2633
2634 doublereal m32 = GetReal(mDef(3, 2));
2635 doublereal m33 = GetReal(mDef(3, 3));
2636 doublereal m31 = GetReal(mDef(3, 1));
2637 doublereal m35 = GetReal(mDef(3, 5));
2638 doublereal m36 = GetReal(mDef(3, 6));
2639 doublereal m34 = GetReal(mDef(3, 4));
2640
2641 doublereal m12 = GetReal(mDef(1, 2));
2642 doublereal m13 = GetReal(mDef(1, 3));
2643 doublereal m11 = GetReal(mDef(1, 1));
2644 doublereal m15 = GetReal(mDef(1, 5));
2645 doublereal m16 = GetReal(mDef(1, 6));
2646 doublereal m14 = GetReal(mDef(1, 4));
2647
2648 doublereal m52 = GetReal(mDef(5, 2));
2649 doublereal m53 = GetReal(mDef(5, 3));
2650 doublereal m51 = GetReal(mDef(5, 1));
2651 doublereal m55 = GetReal(mDef(5, 5));
2652 doublereal m56 = GetReal(mDef(5, 6));
2653 doublereal m54 = GetReal(mDef(5, 4));
2654
2655 doublereal m62 = GetReal(mDef(6, 2));
2656 doublereal m63 = GetReal(mDef(6, 3));
2657 doublereal m61 = GetReal(mDef(6, 1));
2658 doublereal m65 = GetReal(mDef(6, 5));
2659 doublereal m66 = GetReal(mDef(6, 6));
2660 doublereal m64 = GetReal(mDef(6, 4));
2661
2662 doublereal m42 = GetReal(mDef(4, 2));
2663 doublereal m43 = GetReal(mDef(4, 3));
2664 doublereal m41 = GetReal(mDef(4, 1));
2665 doublereal m45 = GetReal(mDef(4, 5));
2666 doublereal m46 = GetReal(mDef(4, 6));
2667 doublereal m44 = GetReal(mDef(4, 4));
2668
2669 m = Mat6x6(
2670 m11, m21, m31, m41, m51, m61,
2671 m12, m22, m32, m42, m52, m62,
2672 m13, m23, m33, m43, m53, m63,
2673 m14, m24, m34, m44, m54, m64,
2674 m15, m25, m35, m45, m55, m65,
2675 m16, m26, m36, m46, m56, m66);
2676
2677 } else {
2678 if (IsKeyWord("matr")) {
2679 /* eat it; not required */
2680 NO_OP;
2681 }
2682
2683 doublereal m11 = GetReal(mDef(1, 1));
2684 doublereal m12 = GetReal(mDef(1, 2));
2685 doublereal m13 = GetReal(mDef(1, 3));
2686 doublereal m14 = GetReal(mDef(1, 4));
2687 doublereal m15 = GetReal(mDef(1, 5));
2688 doublereal m16 = GetReal(mDef(1, 6));
2689
2690 doublereal m21 = GetReal(mDef(2, 1));
2691 doublereal m22 = GetReal(mDef(2, 2));
2692 doublereal m23 = GetReal(mDef(2, 3));
2693 doublereal m24 = GetReal(mDef(2, 4));
2694 doublereal m25 = GetReal(mDef(2, 5));
2695 doublereal m26 = GetReal(mDef(2, 6));
2696
2697 doublereal m31 = GetReal(mDef(3, 1));
2698 doublereal m32 = GetReal(mDef(3, 2));
2699 doublereal m33 = GetReal(mDef(3, 3));
2700 doublereal m34 = GetReal(mDef(3, 4));
2701 doublereal m35 = GetReal(mDef(3, 5));
2702 doublereal m36 = GetReal(mDef(3, 6));
2703
2704 doublereal m41 = GetReal(mDef(4, 1));
2705 doublereal m42 = GetReal(mDef(4, 2));
2706 doublereal m43 = GetReal(mDef(4, 3));
2707 doublereal m44 = GetReal(mDef(4, 4));
2708 doublereal m45 = GetReal(mDef(4, 5));
2709 doublereal m46 = GetReal(mDef(4, 6));
2710
2711 doublereal m51 = GetReal(mDef(5, 1));
2712 doublereal m52 = GetReal(mDef(5, 2));
2713 doublereal m53 = GetReal(mDef(5, 3));
2714 doublereal m54 = GetReal(mDef(5, 4));
2715 doublereal m55 = GetReal(mDef(5, 5));
2716 doublereal m56 = GetReal(mDef(5, 6));
2717
2718 doublereal m61 = GetReal(mDef(6, 1));
2719 doublereal m62 = GetReal(mDef(6, 2));
2720 doublereal m63 = GetReal(mDef(6, 3));
2721 doublereal m64 = GetReal(mDef(6, 4));
2722 doublereal m65 = GetReal(mDef(6, 5));
2723 doublereal m66 = GetReal(mDef(6, 6));
2724
2725 m = Mat6x6(
2726 m11, m21, m31, m41, m51, m61,
2727 m12, m22, m32, m42, m52, m62,
2728 m13, m23, m33, m43, m53, m63,
2729 m14, m24, m34, m44, m54, m64,
2730 m15, m25, m35, m45, m55, m65,
2731 m16, m26, m36, m46, m56, m66);
2732 }
2733
2734 if (IsKeyWord("scale")) {
2735 m *= GetReal(1.);
2736 }
2737
2738 return m;
2739 }
2740
2741 /* provvisoria */
2742 void
GetMat6xN(Mat3xN & m1,Mat3xN & m2,integer iNumCols)2743 MBDynParser::GetMat6xN(Mat3xN& m1, Mat3xN& m2, integer iNumCols)
2744 {
2745 ASSERT(iNumCols > 0);
2746 ASSERT(m1.iGetNumCols() == iNumCols);
2747 ASSERT(m2.iGetNumCols() == iNumCols);
2748
2749 if (IsKeyWord("null")) {
2750 m1.Reset();
2751 m2.Reset();
2752
2753 } else {
2754 int vi[] = { 1, 2, 3 };
2755
2756 if (IsKeyWord("anba")) {
2757 vi[0] = 2;
2758 vi[1] = 3;
2759 vi[2] = 1;
2760
2761 } else if (IsKeyWord("matr")) {
2762 /* eat it; not required */
2763 NO_OP;
2764 }
2765
2766 for (int i = 0; i < 3; i++) {
2767 for (integer j = 1; j <= iNumCols; j++) {
2768 m1.Put(vi[i], j, GetReal());
2769 }
2770 }
2771
2772 for (int i = 0; i < 3; i++) {
2773 for (integer j = 1; j <= iNumCols; j++) {
2774 m2.Put(vi[i], j, GetReal());
2775 }
2776 }
2777
2778 if (IsKeyWord("scale")) {
2779 doublereal d = GetReal(1.);
2780
2781 m1 *= d;
2782 m2 *= d;
2783 }
2784 }
2785 }
2786
2787 Vec3
Get(const Vec3 & v)2788 MBDynParser::Get(const Vec3& v)
2789 {
2790 bool bManip = false;
2791 Vec3 vv = v;
2792
2793 while (!bEmptyManip()) {
2794 const Manip *m = GetManip();
2795 const TplManip<Vec3> *v3m = dynamic_cast<const TplManip<Vec3> *>(m);
2796 if (v3m) {
2797 bManip = true;
2798 vv = v3m->Get(vv);
2799 }
2800
2801 PopManip();
2802 }
2803
2804 if (!bManip) {
2805 vv = GetVec3(v);
2806 }
2807
2808 return vv;
2809 }
2810
2811 Mat3x3
Get(const Mat3x3 & m)2812 MBDynParser::Get(const Mat3x3& m)
2813 {
2814 return GetMat3x3(m);
2815 }
2816
2817 Vec6
Get(const Vec6 & v)2818 MBDynParser::Get(const Vec6& v)
2819 {
2820 return GetVec6(v);
2821 }
2822
2823 Mat6x6
Get(const Mat6x6 & m)2824 MBDynParser::Get(const Mat6x6& m)
2825 {
2826 return GetMat6x6(m);
2827 }
2828
2829 template <class T>
~TplManip(void)2830 MBDynParser::TplManip<T>::~TplManip(void)
2831 {
2832 NO_OP;
2833 }
2834
2835 template <class T>
RefTplManip(MBDynParser & HP,const ReferenceFrame & rf,VecMatOpType type)2836 MBDynParser::RefTplManip<T>::RefTplManip(
2837 MBDynParser& HP,
2838 const ReferenceFrame& rf,
2839 VecMatOpType type)
2840 : HP(HP),
2841 rf(rf),
2842 type(type)
2843 {
2844 NO_OP;
2845 }
2846
2847 template <class T>
~RefTplManip(void)2848 MBDynParser::RefTplManip<T>::~RefTplManip(void)
2849 {
2850 NO_OP;
2851 }
2852
RefVec3Manip(MBDynParser & HP,const ReferenceFrame & rf,VecMatOpType type)2853 MBDynParser::RefVec3Manip::RefVec3Manip(
2854 MBDynParser& HP,
2855 const ReferenceFrame& rf,
2856 VecMatOpType type)
2857 : RefTplManip<Vec3>(HP, rf, type)
2858 {
2859 NO_OP;
2860 }
2861
~RefVec3Manip(void)2862 MBDynParser::RefVec3Manip::~RefVec3Manip(void)
2863 {
2864 NO_OP;
2865 }
2866
2867 #if 0
2868 Vec6
2869 MBDynParser::RefVec6Manip::Get(const Vec6& v) const
2870 {
2871 switch (type) {
2872 case VM_NONE:
2873 case VM_POSREL:
2874 case VM_POSABS:
2875 case VM_VELREL:
2876 case VM_VELABS:
2877 case VM_OMEREL:
2878 case VM_OMEABS:
2879 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
2880
2881 case VM_VECREL: {
2882 ReferenceFrame rfOut;
2883 switch (HP.GetRef(rfOut)) {
2884 case GLOBAL: {
2885 Vec6 v(HP.GetVec6());
2886 return MultRV(v, rf.GetR());
2887 }
2888
2889 case UNKNOWNFRAME: /* global */
2890 case NODE:
2891 return HighParser::Get(t);
2892
2893 case LOCAL: {
2894 Mat3x3 R(GetMatR2vec());
2895 Vec6 v(HighParser::Get(t)());
2896 return Vec6(R*v.GetVec1(), R*v.GetVec2());
2897 }
2898
2899 case REFERENCE: {
2900 Vec6 v(HighParser::Get(t)());
2901 return Vec6(rf.GetR().MulTV(rfOut.GetR()*v.GetVec1()),
2902 rf.GetR().MulTV(rfOut.GetR()*v.GetVec2()));
2903 }
2904
2905 case OTHER_POSITION:
2906 case OTHER_ORIENTATION:
2907 case OTHER_NODE:
2908 silent_cerr("Get<Vec6>: \"other\" meaningless in this context "
2909 "at line " << GetLineData() << std::endl);
2910 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
2911
2912 default:
2913 // will error out later
2914 break;
2915 }
2916 } break;
2917
2918 case VM_VECABS: {
2919 ReferenceFrame rfOut;
2920 switch (GetRef(rfOut)) {
2921 case UNKNOWNFRAME: /* global */
2922 case GLOBAL:
2923 return HighParser::Get(t)();
2924
2925 case NODE: {
2926 Vec6 v(HighParser::Get(t)());
2927 return Vec6(rf.GetR()*v.GetVec1(), rf.GetR()*v.GetVec2());
2928 }
2929
2930 case LOCAL: {
2931 Mat3x3 R(GetMatR2vec());
2932 Vec6 v(HighParser::Get(t)());
2933 return Vec6(rf.GetR()*(R*v.GetVec1()), rf.GetR()*(R*v.GetVec2()));
2934 }
2935
2936 case REFERENCE: {
2937 Vec6 v(HighParser::Get(t)());
2938 return Vec6(rfOut.GetR()*v.GetVec1(), rfOut.GetR()*v.GetVec2());
2939 }
2940
2941 case OTHER_POSITION:
2942 case OTHER_ORIENTATION:
2943 case OTHER_NODE:
2944 silent_cerr("Get<Vec6>: \"other\" meaningless in this context "
2945 "at line " << GetLineData() << std::endl);
2946 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
2947
2948 default:
2949 // will error out later
2950 break;
2951 }
2952 } break;
2953
2954 case VM_UNITVECREL:
2955 case VM_UNITVECABS:
2956 // fall thru
2957
2958 case VM_MATREL:
2959 case VM_MATABS:
2960 case VM_ROTREL:
2961 case VM_ROTABS:
2962 // fall thru
2963
2964 case VM_LAST:
2965 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
2966 }
2967
2968 ASSERTMSG(0, "You shouldn't have reached this point");
2969 throw MBDynParser::ErrGeneric(MBDYN_EXCEPT_ARGS);
2970 }
2971 #endif
2972
2973 Vec3
Get(const Vec3 & v) const2974 MBDynParser::RefVec3Manip::Get(const Vec3& v) const
2975 {
2976 switch (type) {
2977 case VM_NONE:
2978 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
2979
2980 case VM_POSREL:
2981 return HP.GetPosRel(rf);
2982
2983 case VM_POSABS:
2984 return HP.GetPosAbs(rf);
2985
2986 case VM_VELREL:
2987 case VM_VELABS:
2988 // need offset
2989 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
2990
2991 case VM_OMEREL:
2992 return HP.GetOmeRel(rf);
2993
2994 case VM_OMEABS:
2995 return HP.GetOmeAbs(rf);
2996
2997 case VM_VECREL:
2998 return HP.GetVecRel(rf);
2999
3000 case VM_VECABS:
3001 return HP.GetVecAbs(rf);
3002
3003 case VM_UNITVECREL:
3004 return HP.GetUnitVecRel(rf);
3005
3006 case VM_UNITVECABS:
3007 return HP.GetUnitVecAbs(rf);
3008
3009 case VM_MATREL:
3010 case VM_MATABS:
3011 case VM_ROTREL:
3012 case VM_ROTABS:
3013 // fall thru
3014
3015 default:
3016 case VM_LAST:
3017 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3018 }
3019
3020 return v;
3021 }
3022
VecRelManip(MBDynParser & HP,const ReferenceFrame & rf)3023 MBDynParser::VecRelManip::VecRelManip(
3024 MBDynParser& HP,
3025 const ReferenceFrame& rf)
3026 : RefVec3Manip(HP, rf, VM_VECREL)
3027 {
3028 NO_OP;
3029 }
3030
~VecRelManip(void)3031 MBDynParser::VecRelManip::~VecRelManip(void)
3032 {
3033 NO_OP;
3034 }
3035
VecAbsManip(MBDynParser & HP,const ReferenceFrame & rf)3036 MBDynParser::VecAbsManip::VecAbsManip(
3037 MBDynParser& HP,
3038 const ReferenceFrame& rf)
3039 : RefVec3Manip(HP, rf, VM_VECABS)
3040 {
3041 NO_OP;
3042 }
3043
~VecAbsManip(void)3044 MBDynParser::VecAbsManip::~VecAbsManip(void)
3045 {
3046 NO_OP;
3047 }
3048
3049 // icc hack
3050 // was needed with older c++ headers; not needed any more
3051 // (actually, causes sigsegv with g++ 4.6.1 headers)
3052 #if 0 // defined(__ICC)
3053 static Table dummy_t(false);
3054 static MathParser dummy_mp(dummy_t);
3055 static InputStream dummy_in(std::cin);
3056 static MBDynParser dummy_hp(dummy_mp, dummy_in, "dummy initialization");
3057
3058 void
3059 MBDynParser_dummy_init(void)
3060 {
3061 TplDriveCaller<doublereal> *dummy_tpldc1d(dummy_hp.GetTplDriveCaller<doublereal>());
3062 TplDriveCaller<Vec3> *dummy_tpldc3d(dummy_hp.GetTplDriveCaller<Vec3>());
3063 TplDriveCaller<Vec6> *dummy_tpldc6d(dummy_hp.GetTplDriveCaller<Vec6>());
3064
3065 TplDriveCaller<Mat3x3> *dummy_tpldc3x3d(dummy_hp.GetTplDriveCaller<Mat3x3>());
3066 TplDriveCaller<Mat6x6> *dummy_tpldc6x6d(dummy_hp.GetTplDriveCaller<Mat6x6>());
3067 }
3068 #endif // __ICC
3069 // end of icc hack
3070
3071 /* MBDynParser - end */
3072
3073