1 //#**************************************************************
2 //#
3 //# filename: mbs.cpp
4 //#
5 //# author: Gerstmayr Johannes
6 //#
7 //# generated: July 2004
8 //# description: Driver and model for timeintegration
9 //# Model of a rigid arm and a hydraulic zylinder
10 //# remarks:
11 //#
12 //# Copyright (c) 2003-2013 Johannes Gerstmayr, Linz Center of Mechatronics GmbH, Austrian
13 //# Center of Competence in Mechatronics GmbH, Institute of Technical Mechanics at the
14 //# Johannes Kepler Universitaet Linz, Austria. All rights reserved.
15 //#
16 //# This file is part of HotInt.
17 //# HotInt is free software: you can redistribute it and/or modify it under the terms of
18 //# the HOTINT license. See folder 'licenses' for more details.
19 //#
20 //# bug reports are welcome!!!
21 //# WWW: www.hotint.org
22 //# email: bug_reports@hotint.org or support@hotint.org
23 //#***************************************************************************************
24
25
26 #include "mbs.h"
27 //#include "windows.h" //for shell execute
28 #include <direct.h> //for getcwd
29 #include <io.h> //file operation _access
30 #include <stdio.h>
31 #include <stdlib.h>
32
33 #include "parser.h"
34 #include "script_parser.h"
35 #include "element.h"
36 #include "sensors.h"
37 #include "node.h"
38 #include "material.h"
39 #include "parser.h"
40
41 #define MBS_use_parallelization_NOT
42
43 #ifdef MBS_use_parallelization
44 #include <omp.h> //for parallelization
45 const int OMP_MAX_N_THREADS=12;
46 #endif
47
48 #include "lapack_routines.h"
49 #include "options_mbs_auto.h"
50
51 //the following file contains the conversion MBSSolSet <==> EDC
52 #include "mbssolset_convert_auto.h"
53
54 extern MultiBodySystem * TIMBS;
55
TIMBSWarningHandle(const char * warn,int use_instant_message_text)56 void TIMBSWarningHandle(const char* warn, int use_instant_message_text)
57 {
58 //$ AD 2011-11: flag may have negative values, then no output
59 if(use_instant_message_text < 0) return;
60
61 if(!use_instant_message_text)
62 TIMBS->UO(UO_LVL_warn) << "WARNING:" << warn << "\n";
63 else
64 TIMBS->UO(UO_LVL_warn).InstantMessageText(mystr("WARNING:")+mystr(warn));
65
66 }
67
68 //AP 2013-01-15: constructor TimeInt() is called explicitely (here *global_uo is set, thus it is essential)
MultiBodySystem()69 MultiBodySystem::MultiBodySystem(): TimeInt(), elements(), nodes(), auxelements(), sensors(), indexOfActualPostProcessingFieldVariable(0), eigval()
70 {
71 maxindex = 2;
72 magnify_yz = 1;
73
74 //++++++++++ parameter variation/optimization
75 varparameter = 1;
76 varparameter2 = 0;
77 ptr2PerformComputation_Function = 0;
78 //++++++++++
79
80 resortsize_add = 0;
81 //evalfcnt = 0;
82 //evalf_jaccnt = 0;
83 //writesolcnt = 0;
84 //testcnt = 0;
85
86 centerobject = 0;
87 centerobject_offset = Vector3D(0.,0.,0.);
88
89 transformJacApply = 0;
90 prohibit_redraw = 0;
91
92 elementbandwidth = 16;
93 use_dependencies = 0;
94
95 isfirstcomputation = 1;
96 isinconsistent = 0;
97 isloadsavemode = 0;
98
99 resort_active = 1;
100
101 edc_modeldata = 0; //empty!
102 modeldata_initialized = 0;
103
104 mbs_edc_options = 0;
105 mbs_edc_global_variables = 0;
106
107 ElementDataContainer edc_init;
108 SetMBS_EDC_Options(edc_init);
109
110 ElementDataContainer edc_variables;
111 SetMBS_EDC_Variables(edc_variables);
112
113 mbs_parser = new CMBSParser();
114 MBSParser().SetMBS(this);
115 MBSParser().Init();
116
117 edc_parser = new CEDCParser();
118 edc_parser->SetMBS(this, mbs_parser);
119 edc_parser->Initialize();
120
121 stored_initialconditions.SetLen(0);
122 isSolutionFileHeaderWritten = 0;
123 isSolParFileHeaderWritten = 0;
124
125 //$ MS + RL 2012-2-29: coreComputationFinished=0; //$ MS 2011-3-16:
126 simulationStatus.SetStatusFlag(TSimulationNotStarted); //$ MS + RL 2012-2-29: //$ MaSch 2013-08-19
127
128 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
129 //this call adds all class .tex descriptions for online or offline access
130 //EDCParser().AddClassDescriptions_Auto();
131
132 serversocket = NULL;
133 }
134
~MultiBodySystem()135 MultiBodySystem::~MultiBodySystem()
136 {
137 //delete everyhing newed!!!!!!!!!!!!!!!!!!!!!!
138 //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
139 Destroy();
140
141 if (mbs_edc_options) delete mbs_edc_options;
142 if (edc_modeldata) delete edc_modeldata;
143 if (mbs_edc_global_variables) delete mbs_edc_global_variables;
144 if (mbs_parser) delete mbs_parser;
145 }
146
147
Destroy()148 void MultiBodySystem::Destroy()
149 {
150 for (int i=1; i <= elements.Length(); i++)
151 {
152 delete elements(i);
153 }
154 for (int i=1; i <= auxelements.Length(); i++)
155 {
156 delete auxelements(i);
157 }
158 for (int i=1; i <= nodes.Length(); i++)
159 {
160 delete nodes(i);
161 }
162 for (int i=1; i <= sensors.Length(); i++)
163 {
164 delete sensors(i);
165 }
166 for (int i=1; i <= loads.Length(); i++) //$ DR 21012-10
167 {
168 delete loads(i);
169 }
170 elements.Flush();
171 nodes.Flush();
172 auxelements.Flush();
173 sensors.Flush();
174 loads.Flush(); //$ DR 21012-10
175
176 for (int i=1; i <= drawelements.Length(); i++)
177 {
178 delete drawelements(i);
179 }
180 drawelements.Flush();
181
182 for (int i=1; i <= materials.Length(); i++)
183 {
184 delete materials(i);
185 }
186 materials.Flush();
187
188 NumSolver().DestroyOldJacs();
189
190 mbs_parser->GetVariableList()->Flush();
191 }
192
InitFirst()193 void MultiBodySystem::InitFirst()
194 {
195 TimeInt::InitFirst();
196
197 //init global variables
198 InitGlobalEDCVariables();
199 }
200
201 //add global variables
InitGlobalEDCVariables()202 void MultiBodySystem::InitGlobalEDCVariables()
203 {
204 ElementDataContainer* edc = GetMBS_EDC_Variables();
205 ElementData ed;
206
207 ed.SetDouble(MY_PI, "pi"); edc->Add(ed);
208 }
209
CheckConsistencyLTG()210 int MultiBodySystem::CheckConsistencyLTG()
211 {
212 //check LTG (due to delete or internal error)
213
214 MBSsos = GetSecondOrderSize();
215 MBSes = GetFirstOrderSize();
216 MBSis = GetImplicitSize();
217 MBSss = GetSystemSize();
218
219 int l2problem = 0; //level 2 problem: can not compute and not draw
220
221 for (int i=1; i<=elements.Length(); i++)
222 {
223 Element* e = elements(i);
224 const TArray<int>& ltg = e->GetLTGArray();
225 if (ltg.Length() != e->SS())
226 {
227 l2problem = 1;
228 UO(UO_LVL_err) << "System inconsistency: Element " << i << " has an inconsistency in the local to global DOF reference numbers!\n";
229 }
230
231 for (int j=1; j<=ltg.Length(); j++)
232 {
233 if (ltg.Get(j) <= 0 || ltg.Get(j) > MBSss)
234 {
235 l2problem = 1;
236 UO(UO_LVL_err) << "System inconsistency: Element " << i << " has invalid local to global DOF reference numbers!\n";
237 }
238 }
239 }
240
241 if(l2problem) return 2;
242 else return 0;
243 }
244
CheckConsistencyElements()245 int MultiBodySystem::CheckConsistencyElements()
246 {
247 int l1problem = 0; //level 1 problem: can not compute
248 int l2problem = 0; //level 2 problem: can not compute and not draw
249 mystr errorstr = "";
250
251 for (int i=1; i<=elements.Length(); i++)
252 {
253 int rv = elements(i)->CheckConsistency(errorstr);
254 if (rv == 1) l1problem = 1;
255 else if (rv == 2) l2problem = 1;
256
257 if (rv)
258 {
259 UO(UO_LVL_err) << "System inconsistency in element " << i << ":\n";
260 UO(UO_LVL_err) << errorstr;
261 errorstr = "";
262 if (rv == 1) l1problem = 1;
263 else if (rv == 2) l2problem = 1;
264 }
265 }
266
267 if (l2problem) return 2;
268 else if (l1problem) return 1;
269
270 return 0;
271 }
272
CheckConsistencySensors()273 int MultiBodySystem::CheckConsistencySensors()
274 {
275 int l1problem = 0; //level 1 problem: can not compute
276 int l2problem = 0; //level 2 problem: can not compute and not draw
277 mystr errorstr = "";
278
279 for (int i=1; i <= NSensors(); i++)
280 {
281 //$ YV 2012-06: for sensors true is consistent and false is not
282 int rv = sensors(i)->IsConsistent(errorstr) ? 0 : 1;
283 if (rv == 1) l1problem = 1;
284 //else if (rv == 2) l2problem = 1;
285
286 if (rv)
287 {
288 UO(UO_LVL_err) << "System inconsistency in sensor " << i << ":\n";
289 UO(UO_LVL_err) << errorstr;
290 errorstr = "";
291 if (rv == 1) l1problem = 1;
292 else if (rv == 2) l2problem = 1;
293 }
294 }
295
296 if (l2problem) return 2;
297 else if (l1problem) return 1;
298
299 return 0;
300 }
301
CheckConsistencyNodes()302 int MultiBodySystem::CheckConsistencyNodes()
303 {
304 int l1problem = 0; //level 1 problem: can not compute
305 int l2problem = 0; //level 2 problem: can not compute and not draw
306 mystr errorstr = "";
307
308 //check if all nodes are used:
309 TArray<int> nodescheck;
310 nodescheck.SetLen(NNodes());
311 for (int i=1; i <= nodescheck.Length(); i++)
312 {
313 nodescheck(i) = 0;
314 }
315 for (int i=1; i<=elements.Length(); i++)
316 {
317 Element* e = elements(i);
318 if (e->PerformNodeCheck())
319 {
320 for (int j=1; j <= e->NNodes(); j++)
321 {
322 //nodenumber is already verified in Element->CheckConsistency, her it is checked if node is used ...
323 if (e->NodeNum(j) > 0 && e->NodeNum(j) <= nodescheck.Length())
324 {
325 nodescheck(e->NodeNum(j)) = 1;
326 }
327 }
328 }
329 }
330 for (int i=1; i <= nodescheck.Length(); i++)
331 {
332 if (nodescheck(i) == 0)
333 {
334 UO(UO_LVL_err) << "System inconsistency in node number " << i << ":\n Node is not used. You need to delete the node, otherwise the computation is not possible!!!\n";
335 l1problem = 1;
336 }
337 }
338
339 if (l2problem) return 2;
340 else if (l1problem) return 1;
341
342 return 0;
343 }
344
CheckConsistencyMaterials()345 int MultiBodySystem::CheckConsistencyMaterials()
346 {
347 int l1problem = 0; //level 1 problem: can not compute
348 int l2problem = 0; //level 2 problem: can not compute and not draw
349 mystr errorstr = "";
350
351 for (int i=1; i<=NMaterials(); i++)
352 {
353 int rv = materials(i)->CheckConsistency(errorstr);
354 if (rv == 1) l1problem = 1;
355 else if (rv == 2) l2problem = 1;
356
357 if (rv)
358 {
359 UO(UO_LVL_err) << "System inconsistency in material " << i << ":\n";
360 UO(UO_LVL_err) << errorstr;
361 errorstr = "";
362 if (rv == 1) l1problem = 1;
363 else if (rv == 2) l2problem = 1;
364 }
365 }
366
367 if (l2problem) return 2;
368 else if (l1problem) return 1;
369
370 return 0;
371 }
372
CheckSystemConsistency()373 int MultiBodySystem::CheckSystemConsistency() //check system; rv==0 --> OK, rv==1 --> can not compute, rv==2 --> can not draw and not compute
374 {
375 //int l1problem = 0; //level 1 problem: can not compute
376 //int l2problem = 0; //level 2 problem: can not compute and not draw
377
378 //++++++++++++++++++++++++++++++++++++++++++++++++++++
379 //first check if assemble and initialize is possible
380 //check general inconsistencies
381 //check LTG (due to delete or internal error)
382
383 //MBSsos = GetSecondOrderSize();
384 //MBSes = GetFirstOrderSize();
385 //MBSis = GetImplicitSize();
386 //MBSss = GetSystemSize();
387 //
388 // for (int i=1; i<=elements.Length(); i++)
389 // {
390 // Element* e = elements(i);
391 // const TArray<int>& ltg = e->GetLTGArray();
392 // if (ltg.Length() != e->SS())
393 // {
394 // l2problem = 1;
395 // UO(UO_LVL_err) << "System inconsistency: Element " << i << " has an inconsistency in the local to global DOF reference numbers!\n";
396 // }
397 //
398 // for (int j=1; j<=ltg.Length(); j++)
399 // {
400 //// (AD) changed () to .Get()
401 // if (ltg.Get(j) <= 0 || ltg.Get(j) > MBSss)
402 //// if (ltg(j) <= 0 || ltg(j) > MBSss)
403 // {
404 // l2problem = 1;
405 // UO(UO_LVL_err) << "System inconsistency: Element " << i << " has invalid local to global DOF reference numbers!\n";
406 // }
407 // }
408 // }
409 //
410 // if (l2problem) return 2;
411
412 int rv = CheckConsistencyLTG();
413 if(rv==2) return 2;
414
415 //now call element consistency check:
416 //mystr errorstr = "";
417 //for (int i=1; i<=elements.Length(); i++)
418 //{
419 // int rv = elements(i)->CheckConsistency(errorstr);
420 // if (rv == 1) l1problem = 1;
421 // else if (rv == 2) l2problem = 1;
422
423 // if (rv)
424 // {
425 // UO(UO_LVL_err) << "System inconsistency in element " << i << ":\n";
426 // UO(UO_LVL_err) << errorstr;
427 // errorstr = "";
428 // if (rv == 1) l1problem = 1;
429 // else if (rv == 2) l2problem = 1;
430 // }
431 //}
432
433 rv = Maximum(rv,CheckConsistencyElements());
434
435 //for (int i=1; i <= NSensors(); i++)
436 //{
437 // //$ YV 2012-06: for sensors true is consistent and false is not
438 // int rv = sensors(i)->IsConsistent(errorstr) ? 0 : 1;
439 // if (rv == 1) l1problem = 1;
440 // //else if (rv == 2) l2problem = 1;
441
442 // if (rv)
443 // {
444 // UO(UO_LVL_err) << "System inconsistency in sensor " << i << ":\n";
445 // UO(UO_LVL_err) << errorstr;
446 // errorstr = "";
447 // if (rv == 1) l1problem = 1;
448 // else if (rv == 2) l2problem = 1;
449 // }
450 //}
451
452 rv = Maximum(rv,CheckConsistencySensors());
453
454 //check if all nodes are used:
455 //TArray<int> nodescheck;
456 //nodescheck.SetLen(NNodes());
457 //for (int i=1; i <= nodescheck.Length(); i++)
458 //{
459 // nodescheck(i) = 0;
460 //}
461 //for (int i=1; i<=elements.Length(); i++)
462 //{
463 // Element* e = elements(i);
464 // if (e->PerformNodeCheck())
465 // {
466 // for (int j=1; j <= e->NNodes(); j++)
467 // {
468 // //nodenumber is already verified in Element->CheckConsistency, her it is checked if node is used ...
469 // if (e->NodeNum(j) > 0 && e->NodeNum(j) <= nodescheck.Length())
470 // {
471 // nodescheck(e->NodeNum(j)) = 1;
472 // }
473 // }
474 // }
475 //}
476 //for (int i=1; i <= nodescheck.Length(); i++)
477 //{
478 // if (nodescheck(i) == 0)
479 // {
480 // UO(UO_LVL_err) << "System inconsistency in node number " << i << ":\n Node is not used. You need to delete the node, otherwise the computation is not possible!!!\n";
481 // l1problem = 1;
482 // }
483 //}
484 rv = Maximum(rv,CheckConsistencyNodes());
485
486 //$ PG 2012-12-19: added consistency check for materials
487 rv = Maximum(rv,CheckConsistencyMaterials());
488
489 if (rv==2) return 2;
490 else if (rv==1) return 1;
491
492 UO() << "System is consistent!\n";
493 return 0;
494
495 //if (l2problem) return 2;
496 //else if (l1problem) return 1;
497
498 //UO() << "System is consistent!\n";
499 //return 0;
500
501 }
502
PrecomputeEvalFunctions()503 void MultiBodySystem::PrecomputeEvalFunctions() //PG: precomputation for EvalF(), EvalF2(), EvalG(), EvalM(), and also for CqTLambda() in case of constraints
504 {
505 //adjusted for IOElementDataModifier by JG 2013-17-01 and MSax
506
507 for (int i=1; i<=elements.Length(); i++)
508 {
509 //first initialize all modifier elements, because these elements modify the data of other elements!
510 if (elements(i)->GetType() & TIOElementDataModifier) elements(i)->PrecomputeEvalFunctions();
511 }
512 for (int i=1; i<=elements.Length(); i++)
513 {
514 if (!(elements(i)->GetType() & TIOElementDataModifier)) elements(i)->PrecomputeEvalFunctions();
515 //elements(i)->StartTimeStep();
516 }
517
518
519 //for (int i=1; i<=elements.Length(); i++)
520 //{
521 // elements(i)->PrecomputeEvalFunctions();
522 //}
523 }
524
525
EvalM(const Vector & x,Matrix & m,double t)526 void MultiBodySystem::EvalM(const Vector& x, Matrix& m, double t)
527 {
528 TMStartTimer(6);
529 SetActState(x);
530
531 //old: set all 0
532 //m.SetAll(0);
533 //new: clear only element entries:
534 int maxbw = 0;
535 for (int i=1; i<=elements.Length(); i++)
536 {
537 Element* e = elements(i);
538 int sos = e->SOS();
539 if (sos)
540 {
541 const TArray<int>& ltg = e->GetLTGArray();
542 for (int j=1; j <= sos; j++)
543 {
544 for (int k=1; k <= sos; k++)
545 {
546 // (AD) changed () to .Get()
547 maxbw = Maximum(maxbw,ltg.Get(j)-ltg.Get(k)+1);
548 m(ltg.Get(j),ltg.Get(k)) = 0;
549 // maxbw = Maximum(maxbw,ltg(j)-ltg(k)+1);
550 // m(ltg(j),ltg(k)) = 0;
551 }
552 }
553 }
554 }
555 SetBWM(maxbw);
556 //UO() << "max Bandwidth=" << maxbw << "\n";
557 for (int i=1; i<=elements.Length(); i++)
558 {
559 Element* e = elements(i);
560 int sos = e->SOS();
561 if (sos)
562 {
563 tempm.SetSize(sos,sos);
564 tempm.FillWithZeros();
565 e->EvalM(tempm,t);
566 //uo << "tempm=" << tempm;
567
568 m.AddMatrix(e->GetLTGArray(), e->GetLTGArray(), tempm);
569
570 /*
571 const TArray<int>& ltg = e->GetLTGArray();
572 for (int j=1; j <= sos; j++)
573 {
574 for (int k=1; k <= sos; k++)
575 {
576 //m(e->LTG(j),e->LTG(k)) += tempm(j,k);
577 m(ltg(j),ltg(k)) += tempm(j,k);
578 }
579 }*/
580 }
581 }
582 TMStopTimer(6);
583 //uo << "m=" << m << "\n";
584 }
585
EvalM(const Vector & x,SparseMatrix & m,double t)586 void MultiBodySystem::EvalM(const Vector& x, SparseMatrix& m, double t)
587 {
588 TMStartTimer(6);
589 SetActState(x);
590
591 m.FillWithZeros();
592
593 for (int i=1; i<=elements.Length(); i++)
594 {
595 Element* e = elements(i);
596
597 if (e->UseSparseM())
598 {
599 e->AddMSparse(m,t);
600 }
601 else
602 {
603 int sos = e->SOS();
604 if (sos)
605 {
606 tempm.SetSize(sos,sos);
607 tempm.FillWithZeros();
608 e->EvalM(tempm,t);
609 const TArray<int>& ltg = e->GetLTGArray();
610 m.AddMatrix(ltg,ltg,sos,sos,tempm);
611 }
612 }
613 }
614 TMStopTimer(6);
615 }
616
EvalF2(const Vector & x,Vector & f,double t)617 void MultiBodySystem::EvalF2(const Vector& x, Vector& f, double t)
618 {
619 TMStartTimer(5);
620 if (NLS_GetJacCol() == 0) {log.evalfcnt++;}
621 if (NLS_GetJacCol() != 0) {log.evalf_jaccnt++;}
622
623 int jc = GetJacCol();
624
625 f.SetAll(0);
626 SetActState(x);
627 for (int i=1; i<=elements.Length(); i++)
628 {
629 Element* e = elements(i);
630 int sos = e->SOS();
631 if (sos)
632 {
633 if(e->IsDependent(jc))
634 {
635 tempf.SetLen(sos);
636 tempf.SetAll(0);
637 e->EvalF2(tempf,t);
638 for (int j=1; j <= sos; j++)
639 {
640 f(e->LTG(j)) += tempf(j);
641 }
642 }
643 }
644 }
645 //uo << "f2=" << f << "\n";
646 TMStopTimer(5);
647 }
648
649 #ifndef MBS_use_parallelization
650 //for rapid Explicit integration (e.g. particles)
EvalMinvF2(const Vector & x,Vector & f,double t)651 void MultiBodySystem::EvalMinvF2(const Vector& x, Vector& f, double t)
652 {
653 TMStartTimer(5);
654 if (NLS_GetJacCol() == 0) {log.evalfcnt++;}
655 if (NLS_GetJacCol() != 0) {log.evalf_jaccnt++;}
656
657 f.SetAll(0);
658 SetActState(x);
659 for (int i=1; i<=elements.Length(); i++)
660 {
661 Element* e = elements(i);
662 int sos = e->SOS();
663 if (sos)
664 {
665 tempf.SetLen(sos);
666 tempf.SetAll(0);
667 e->EvalMinvF2(tempf,t);
668 for (int j=1; j <= sos; j++)
669 {
670 f(e->LTG(j)) += tempf(j);
671 }
672 }
673 }
674 //uo << "f2=" << f << "\n";
675 TMStopTimer(5);
676 }
677 #endif
678
679 #ifdef MBS_use_parallelization
680 //for rapid Explicit integration (e.g. particles)
EvalMinvF2(const Vector & x,Vector & f,double t)681 void MultiBodySystem::EvalMinvF2(const Vector& x, Vector& f, double t)
682 {
683 TMStartTimer(5);
684 if (NLS_GetJacCol() == 0) {evalfcnt++;}
685 if (NLS_GetJacCol() != 0) {evalf_jaccnt++;}
686
687 f.SetAll(0);
688 SetActState(x);
689
690 //omp_set_num_threads(MBSSolSet().parallel_number_of_threads);
691 omp_set_num_threads(2);
692
693 int i,j;
694
695 Vector tempf[OMP_MAX_N_THREADS];
696 int nt;
697 int ne = elements.Length();
698 //#pragma omp parallel for private(nt,j,sos,e)
699 /*
700 double a[OMP_MAX_N_THREADS];
701 a[0]=0;a[1]=0;a[2]=0;a[3]=0;
702 double b=0;
703 int n = 20000;
704
705
706 //#pragma omp parallel shared(tempf, elements, f, t, ne, a, b) private(i,j,nt)
707 #pragma omp parallel for shared(a) private(nt,j)
708 for (i=1; i<=n; i++)
709 {
710 nt = omp_get_thread_num();
711 for (j=1; j<= 100; j++)
712 {
713 a[nt] += sin((double)j*(double)i);
714 }
715 }
716 */
717
718
719 #pragma omp parallel shared(tempf, f, t, ne) private(nt, j)
720 {
721 #pragma omp for
722 for (i=1; i<=ne; i++)
723 {
724 nt = omp_get_thread_num();
725 if (elements(i)->SOS())
726 {
727 tempf[nt].SetLen(elements(i)->SOS());
728 tempf[nt].SetAll(0);
729 elements(i)->EvalMinvF2(tempf[nt],t);
730 for (j=1; j <= elements(i)->SOS(); j++)
731 {
732 f(elements(i)->LTG(j)) += tempf[nt](j);
733 }
734 }
735 }
736 } //End of parallel region
737
738 /*
739 for (i=1; i<=ne; i++)
740 {
741 int nt = 0;//omp_get_thread_num();
742 if (elements(i)->SOS())
743 {
744 tempf[nt].SetLen(elements(i)->SOS());
745 tempf[nt].SetAll(0);
746 elements(i)->EvalMinvF2(tempf[nt],t);
747 //tempf[nt] *= a[nt];
748 for (j=1; j <= elements(i)->SOS(); j++)
749 {
750 f(elements(i)->LTG(j)) += tempf[nt](j);
751 }
752 }
753 }
754 */
755 //uo << "f2=" << f << "\n";
756 TMStopTimer(5);
757 }
758 #endif
759
EvalF2(const Vector & x,SparseVector & f,double t,IVector & rowind,IVector & clearind,IVector & elems)760 void MultiBodySystem::EvalF2(const Vector& x, SparseVector& f, double t, IVector& rowind, IVector& clearind, IVector& elems)
761 {
762 //rowind has f.Length() is initialized with zeros and returned with zeros!
763 //clearind is arbitrary and contains the indices of rowind to be set 0
764 //rowind and clearind is used to fastly access the sparsevector f
765
766 //elems is used to see if the second vector for numerical differentiation needs to be computed
767
768 TMStartTimer(5);
769 TMStartTimer(9);
770
771 clearind.SetLen(0);
772 f.FillWithZeros();
773 elems.SetLen(0);
774
775 if (NLS_GetJacCol() == 0) {log.evalfcnt++;}
776 if (NLS_GetJacCol() != 0) {log.evalf_jaccnt++;}
777 int ltg;
778
779 int jc = GetJacCol();
780
781
782 for (int i=1; i<=elements.Length(); i++)
783 {
784 const Element& e = *elements(i);
785 int sos = e.SOS();
786 if (sos)
787 {
788 if(e.IsDependent(jc))
789 {
790 elems.Add(i);
791 for (int j=1; j <= sos; j++)
792 {
793 ltg = e.LTG(j);
794 if (!rowind(ltg))
795 {
796 clearind.Add(ltg);
797 f.AddEntry(ltg,0);
798 rowind(ltg) = f.NEntries();
799 }
800 }
801 }
802 }
803 }
804 TMStopTimer(9);
805
806 SetActState(x);
807 for (int i=1; i<=elements.Length(); i++)
808 {
809 Element* e = elements(i);
810 int sos = e->SOS();
811 if (sos)
812 {
813 if(e->IsDependent(jc))
814 {
815 tempf.SetLen(sos);
816 tempf.SetAll(0);
817 e->EvalF2(tempf,t);
818 for (int j=1; j <= sos; j++)
819 {
820 f.Entry(rowind(e->LTG(j))) += tempf(j);
821 //f(e->LTG(j)) += tempf(j);
822 }
823 }
824 }
825 }
826
827 for (int i=1; i <= clearind.Length(); i++) rowind(clearind(i)) = 0;
828
829 TMStopTimer(5);
830 }
831
832 #define SpecialAdd Add //AddIfNotExists
833 //compute entries of each row in sparse matrix
ComputeSparseMatrixUsage(IVector & usage_per_dof)834 void MultiBodySystem::ComputeSparseMatrixUsage(IVector& usage_per_dof)
835 {
836 int sos= GetSecondOrderSize();//Number of 2nd order ODEs
837 int es = GetFirstOrderSize(); //Number of 1st order ODEs
838 int is = GetImplicitSize(); //Algebraic Equations (from boundary conditions)
839 int matsize = sos+es+is;
840 int init_col_size=4; //initial size of columns
841
842 usage_per_dof.SetLen(matsize);
843 usage_per_dof.SetAll(0);
844
845 TArray<IVector*> usage_per_dof_matrix(sos+es+is);
846 for (int i=1; i<=matsize; i++)
847 {
848 usage_per_dof_matrix(i) = new IVector(init_col_size);
849 }
850
851 //this function is called before computation of sparse stiffness or mass matrix
852 for (int i=1; i<=elements.Length(); i++)
853 {
854 Element* e = elements(i);
855
856 int elem_sos = e->SOS();
857 int elem_es = e->ES();
858 int elem_is = e->IS();
859 int elem_ss = elem_sos+elem_es+elem_is;
860
861 const TArray<int>& ltgrows = e->GetLTGArray();
862
863 //UO(UO_LVL_all) << "elem" << i << ": n=" << ltgrows.Length() << ", ltg=" << ltgrows << "\n";
864
865 //classical stiffness terms:
866 for (int j=1; j <= elem_sos; j++)
867 {
868 int ind = ltgrows(j);
869 usage_per_dof(ind) += elem_ss;
870
871 IVector* updm = usage_per_dof_matrix(ind);
872 for (int k=1; k <= elem_sos; k++)
873 {
874 updm->SpecialAdd(ltgrows(k));
875 }
876 for (int k=elem_sos*2+1; k <= elem_ss+elem_sos; k++)
877 {
878 updm->SpecialAdd(ltgrows(k));
879 }
880 }
881 //stiffness terms from first order equations:
882 for (int j=1; j <= elem_es; j++)
883 {
884 int ind = ltgrows(elem_sos*2+j)-sos;
885 usage_per_dof(ind) += elem_ss;
886
887 IVector* updm = usage_per_dof_matrix(ind);
888 for (int k=1; k <= elem_sos; k++)
889 {
890 updm->SpecialAdd(ltgrows(k));
891 }
892 for (int k=elem_sos*2+1; k <= elem_ss+elem_sos; k++)
893 {
894 updm->SpecialAdd(ltgrows(k));
895 }
896 }
897 //constraint jacobian matrices:
898 for (int j=1; j <= elem_is; j++)
899 {
900 int ind = ltgrows(elem_sos*2+elem_es+j)-sos;
901 usage_per_dof(ind) += elem_ss;
902
903 IVector* updm = usage_per_dof_matrix(ind);
904 for (int k=1; k <= elem_sos; k++)
905 {
906 updm->SpecialAdd(ltgrows(k));
907 }
908 for (int k=elem_sos*2+1; k <= elem_ss+elem_sos; k++)
909 {
910 updm->SpecialAdd(ltgrows(k));
911 }
912
913 for (int k=1; k <= e->NE(); k++)
914 {
915 //UO(UO_LVL_all) << " elemnum=" << e->GetElnum(k) << "\n";
916 Element* ce = GetElementPtr(e->GetElnum(k));
917 int celem_sos = ce->SOS();
918 int celem_es = ce->ES();
919 int celem_is = ce->IS();
920 int celem_ss = celem_sos+celem_es+celem_is;
921
922 int ind = ltgrows(elem_sos*2+elem_es+j)-sos;
923 usage_per_dof(ind) += celem_ss;
924
925 const TArray<int>& cltgrows = ce->GetLTGArray();
926 IVector* updm = usage_per_dof_matrix(ind);
927 for (int k=1; k <= celem_sos; k++)
928 {
929 updm->SpecialAdd(cltgrows(k));
930 }
931 for (int k=celem_sos*2+1; k <= celem_ss+celem_sos; k++)
932 {
933 updm->SpecialAdd(cltgrows(k));
934 }
935 }
936 }
937 if (e->IsType(TConstraint))
938 {
939 //UO(UO_LVL_all) << "elem" << i << ", NE=" << e->NE() << "\n";
940 for (int k=1; k <= e->NE(); k++)
941 {
942 //UO(UO_LVL_all) << " elemnum=" << e->GetElnum(k) << "\n";
943 Element* ce = GetElementPtr(e->GetElnum(k));
944 int celem_sos = ce->SOS();
945 int celem_es = ce->ES();
946 int celem_is = ce->IS();
947 int celem_ss = celem_sos+celem_es+celem_is;
948
949 const TArray<int>& cltgrows = ce->GetLTGArray();
950
951 for (int m=1; m <= celem_sos; m++)
952 {
953 int ind = cltgrows(m);
954 usage_per_dof(ind) += elem_is;
955
956 IVector* updm = usage_per_dof_matrix(ind);
957 for (int k=elem_sos*2+elem_es+1; k <= elem_ss+elem_sos; k++)
958 {
959 updm->SpecialAdd(ltgrows(k));
960 }
961 }
962 for (int m=1; m <= celem_es; m++)
963 {
964 int ind = cltgrows(celem_sos*2+m)-sos;
965 usage_per_dof(ind) += elem_is;
966
967 IVector* updm = usage_per_dof_matrix(ind);
968 for (int k=elem_sos*2+elem_es+1; k <= elem_ss+elem_sos; k++)
969 {
970 updm->SpecialAdd(ltgrows(k));
971 }
972 }
973 for (int m=1; m <= celem_is; m++)
974 {
975 int ind = cltgrows(celem_sos*2+celem_es+m)-sos;
976 usage_per_dof(ind) += elem_is;
977
978 IVector* updm = usage_per_dof_matrix(ind);
979 for (int k=elem_sos*2+elem_es+1; k <= elem_ss+elem_sos; k++)
980 {
981 updm->SpecialAdd(ltgrows(k));
982 }
983 }
984 }
985 }
986
987 }
988
989 int totalsize = 0;
990 int totalsize2 = 0;
991 for (int i=1; i <= usage_per_dof.Length(); i++)
992 {
993 totalsize += usage_per_dof(i);
994 RemoveRedundantEntries(*usage_per_dof_matrix(i), 1);
995 totalsize2 += usage_per_dof_matrix(i)->Length();
996
997 usage_per_dof(i) = usage_per_dof_matrix(i)->Length();
998 }
999
1000 //UO(UO_LVL_all) << "ComputeSparseMatrixUsage=" << totalsize << "\n";
1001
1002 //UO(UO_LVL_all) << "ComputeSparseMatrixUsage_Exact=" << totalsize2 << "\n";
1003
1004 //delete allocated arrays:
1005 for (int i=1; i<=matsize; i++)
1006 {
1007 delete usage_per_dof_matrix(i);
1008 }
1009 }
1010
ComputeGyroscopicMatrix(SparseMatrix & gy)1011 void MultiBodySystem::ComputeGyroscopicMatrix(SparseMatrix& gy) // $ MSax 2013-07-25 : added
1012 {
1013 //int is = GetImplicitSize();
1014 //int es = GetFirstOrderSize();
1015 int sos = GetSecondOrderSize();
1016 //int ss = GetSystemSize()
1017
1018 SparseMatrix locgy;
1019 Matrix fulllocgy;
1020 gy.SetSize(sos,sos,MaxSparseBandwidth()*2);
1021 gy.FillWithZeros();
1022
1023 for (int i=1; i<=elements.Length(); i++)
1024 {
1025 Element* e = elements(i);
1026
1027 int sos = e->SOS();
1028 if (sos)
1029 {
1030 e->GyroscopicMatrix(locgy);
1031 if (locgy.CountEntries() != 0) // fill in to global gyroscopic matrix (only if not empty)
1032 {
1033 const TArray<int>& ltgrows = e->GetLTGArray();
1034 locgy.GetMatrix(fulllocgy);
1035 gy.AddMatrix(ltgrows,ltgrows,sos,sos,fulllocgy);
1036 }
1037 }
1038 }
1039 }
1040
LocalJacobianF2(SparseMatrix & m,Vector & x)1041 void MultiBodySystem::LocalJacobianF2(SparseMatrix& m, Vector& x)
1042 {
1043 int mode = 2; //1=old mode, 2=element-wise mode
1044 if ((!numsol.ModifiedNewtonActive() || !GetSolSet().nls_modifiednewton) && !(GetSolSet().nls_usesparsesolver))
1045 {
1046 mode = 1;
1047 }
1048
1049 if (mode == 1) UO() << "*****************\nwarning: switched MBS:LocalJacobianF2 to old mode\n******************\n";
1050
1051 double t = GetTime() + 0.5*GetStepSize();
1052 //UO() << "localjacf2\n";
1053
1054 if (mode == 1)
1055 {
1056 TimeInt::LocalJacobianF2(m,x);
1057 }
1058 else
1059 {
1060 TMStartTimer(5); // otherwise EvalF2 not right counted
1061 SetActState(x);
1062
1063 m.FillWithZeros();
1064
1065 for (int i=1; i<=elements.Length(); i++)
1066 {
1067 Element* e = elements(i);
1068
1069 int sos = e->SOS();
1070 if (sos)
1071 {
1072 if (e->UseSparseK())
1073 {
1074 e->AddKSparse(m,t); //follower forces are missing?!!!
1075
1076 //e->AddDSparse(m,t);
1077
1078 //only constraint part:
1079 e->JacobianF2(t,tempm,colref);
1080
1081 const TArray<int>& ltgrows = e->GetLTGArray();
1082 m.AddMatrix(ltgrows,colref,sos,colref.Length(),tempm);
1083 }
1084 else
1085 {
1086
1087 e->JacobianF2(t,tempm,colref);
1088 const TArray<int>& ltgrows = e->GetLTGArray();
1089 m.AddMatrix(ltgrows,colref,sos,colref.Length(),tempm);
1090 }
1091 }
1092 }
1093 TMStopTimer(5);
1094
1095 //int n = m.CountEntries();
1096 //UO() << "K-alloc=" << m.GetLAlloc()*8./1.e6 << "MB, entries=" << (double)n/1e6 << "\n";
1097
1098 }
1099
1100 }
1101
StaticJacobianF2(SparseMatrix & m,Vector & x)1102 void MultiBodySystem::StaticJacobianF2(SparseMatrix& m, Vector& x)
1103 {
1104 double t = GetTime() + 0.5*GetStepSize();
1105 //UO() << "localjacf2\n";
1106
1107 TMStartTimer(5); // otherwise EvalF2 not right counted
1108 SetActState(x);
1109
1110 int fullsos = GetSecondOrderSize();
1111
1112 m.FillWithZeros();
1113 //TArray<int> ltg_static; //this is the ltg-array, but velocity part is deleted
1114 TArray<int> colref_static; //this is the colref-array, but velocity part is deleted
1115 Matrix tempmnew;
1116
1117 for (int i=1; i<=elements.Length(); i++)
1118 {
1119 Element* e = elements(i);
1120
1121 int sos = e->SOS();
1122 int es = e->ES();
1123 int is = e->IS();
1124 if (sos)
1125 {
1126 //the following function should be extended by e->UseSparseK(), see LocalJacobianF2(sparse...
1127 if (e->UseSparseK())
1128 {
1129 assert(0);
1130 }
1131
1132 colref_static.SetLen(0);
1133
1134 e->JacobianF2(t,tempm,colref);
1135 const TArray<int>& ltgrows = e->GetLTGArray();
1136 for (int j=1; j <= colref.Length(); j++)
1137 {
1138 if (colref(j) <= fullsos)
1139 {
1140 colref_static.Add(colref(j)); //first part of colref should be same as e->ltg
1141 }
1142 else if (colref(j) > 2*fullsos)
1143 {
1144 colref_static.Add(colref(j)-fullsos); //first part of colref should be same as e->ltg
1145 }
1146 }
1147 //for (int j=2*sos+1; j <= colref.Length(); j++)
1148 //{
1149 // colref_static.Add(colref(j)-fullsos);
1150 //}
1151 //ltg_static not needed!
1152 tempmnew.SetSize(tempm.Getrows(), tempm.Getcols()-sos);
1153 tempmnew.SetAll(0.);
1154 tempmnew.AddSubmatrix(tempm,1,1,1,1,tempm.Getrows(), sos, 1.);
1155 tempmnew.AddSubmatrix(tempm,1,1+2*sos,1,1+sos,tempm.Getrows(), tempm.Getcols()-2*sos, 1.);
1156
1157 m.AddMatrix(ltgrows,colref_static,sos,colref_static.Length(),tempmnew);
1158 }
1159 }
1160 TMStopTimer(5);
1161 }
1162
LocalJacobianF2(Matrix & m,Vector & x)1163 void MultiBodySystem::LocalJacobianF2(Matrix& m, Vector& x)
1164 {
1165 int mode = 2; //1=old mode, 2=element-wise mode
1166 if (!UseJacobianElementWise()) mode = 1;
1167
1168 double t = GetTime() + 0.5*GetStepSize();
1169
1170 //UO() << "localjacf2\n";
1171
1172 if (mode == 1)
1173 {
1174 TimeInt::LocalJacobianF2(m,x);
1175 }
1176 else
1177 {
1178 SetActState(x);
1179
1180 m.FillWithZeros();
1181
1182 TMStartTimer(5); // otherwise EvalF2 not right counted
1183 for (int i=1; i<=elements.Length(); i++)
1184 {
1185 Element* e = elements(i);
1186 int sos = e->SOS();
1187 if (sos)
1188 {
1189 e->JacobianF2(t,tempm,colref);
1190 //UO() << "elem=" << i << ": JacF2=" << tempm << "\n";
1191 const TArray<int>& ltgrows = e->GetLTGArray();
1192
1193 for (int j=1; j <= sos; j++)
1194 {
1195 for (int k=1; k <= colref.Length(); k++)
1196 {
1197 // (AD) changed () to .Get()
1198 m(ltgrows.Get(j),colref.Get(k)) += tempm(j,k);
1199 // m(ltgrows(j),colref(k)) += tempm(j,k);
1200 }
1201 }
1202 }
1203 }
1204 TMStopTimer(5);
1205 }
1206 }
1207
1208
1209
1210
LocalJacobianG(SparseMatrix & m,Vector & x)1211 void MultiBodySystem::LocalJacobianG(SparseMatrix& m, Vector& x)
1212 {
1213 int mode = 2; //1=old mode, 2=element-wise mode
1214 double t = GetTime() + 0.5*GetStepSize();
1215 if (mode == 1)
1216 {
1217 TimeInt::LocalJacobianG(m,x);
1218 }
1219 else
1220 {
1221 static IVector rowref(100);
1222 int mbsoff = GetSecondOrderSize()*2+GetFirstOrderSize();
1223
1224 SetActState(x);
1225
1226 m.FillWithZeros();
1227
1228 TMStartTimer(8); // otherwise EvalF2 not right counted
1229 for (int i=1; i<=elements.Length(); i++)
1230 {
1231 rowref.SetLen(0);
1232 Element* e = elements(i);
1233 int is = e->IS();
1234 if (is)
1235 {
1236 e->JacobianG(t,tempm,colref);
1237 const TArray<int>& ltgrows = e->GetLTGArray();
1238 for (int j=1; j <= is; j++)
1239 {
1240 rowref.Add(ltgrows.Get(j+2*e->SOS()+e->ES())-mbsoff);
1241 }
1242 m.AddMatrix(rowref,colref,is,colref.Length(),tempm);
1243 }
1244 }
1245 TMStopTimer(8);
1246 }
1247
1248 }
1249
1250
1251
1252
EvalF(const Vector & x,Vector & f,double t)1253 void MultiBodySystem::EvalF(const Vector& x, Vector& f, double t)
1254 {
1255 TMStartTimer(7);
1256 f.SetAll(0);
1257 int off = -2*MBSsos;
1258 SetActState(x);
1259 int jc = GetJacCol();
1260 for (int i=1; i<=elements.Length(); i++)
1261 {
1262 Element* e = elements(i);
1263 int es = e->ES();
1264 if (es)
1265 {
1266 if (e->IsDependent(jc))
1267 {
1268 int off2=2*e->SOS();
1269 tempf.SetLen(es);
1270 tempf.SetAll(0);
1271 e->EvalF(tempf,t);
1272 for (int j=1; j <= es; j++)
1273 {
1274 f(e->LTG(j+off2)+off) = tempf(j);
1275 }
1276 }
1277 }
1278 }
1279 TMStopTimer(7);
1280 }
1281
EvalG(const Vector & x,Vector & f,double t)1282 void MultiBodySystem::EvalG(const Vector& x, Vector& f, double t)
1283 {
1284 TMStartTimer(8);
1285 int jc = GetJacCol();
1286
1287 f.SetAll(0);
1288 int off = -2*MBSsos-MBSes;
1289 SetActState(x);
1290 for (int i=1; i<=elements.Length(); i++)
1291 {
1292 Element* e = elements(i);
1293 int is = e->IS();
1294 if (is)
1295 {
1296 if (e->IsDependent(jc))
1297 {
1298 int off2=2*e->SOS()+e->ES();
1299 tempf.SetLen(is);
1300 tempf.SetAll(0);
1301 e->EvalG(tempf,t);
1302 for (int j=1; j <= is; j++)
1303 {
1304 f(e->LTG(j+off2)+off) = tempf(j);
1305 }
1306 }
1307 }
1308 }
1309 //uo << "g=" << f << "\n";
1310
1311 //f*=-2./(GetStepSize()); //2./StepSize() originates from KVersion time integration with acceleration instead of position, sign originates because J=-M-tau^2/4*K-CqT L
1312
1313 TMStopTimer(8);
1314 }
1315
EvalG(const Vector & x,SparseVector & f,double t,IVector & rowind,IVector & clearind)1316 void MultiBodySystem::EvalG(const Vector& x, SparseVector& f, double t, IVector& rowind, IVector& clearind)
1317 {
1318 //rowind has f.Length() is initialized with zeros and returned with zeros!
1319 //clearind is arbitrary and contains the indices of rowind to be set 0
1320 TMStartTimer(8);
1321 TMStartTimer(9);
1322
1323 clearind.SetLen(0);
1324 f.FillWithZeros();
1325
1326 int off = -2*MBSsos-MBSes;
1327
1328 //if (NLS_GetJacCol() == 0) {evalfcnt++;}
1329 //if (NLS_GetJacCol() != 0) {evalf_jaccnt++;}
1330
1331 int ltg, off2;
1332
1333 int jc = GetJacCol();
1334
1335 for (int i=1; i<=elements.Length(); i++)
1336 {
1337 Element* e = elements(i);
1338 int is = e->IS();
1339 if (is)
1340 {
1341 if (e->IsDependent(jc))
1342 {
1343 off2=2*e->SOS()+e->ES();
1344 tempf.SetLen(is);
1345 tempf.SetAll(0);
1346 e->EvalG(tempf,t);
1347 for (int j=1; j <= is; j++)
1348 {
1349 ltg = e->LTG(j+off2)+off;
1350 clearind.Add(ltg);
1351 f.AddEntry(ltg,0);
1352 rowind(ltg) = f.NEntries();
1353 }
1354 }
1355 }
1356 }
1357 TMStopTimer(9);
1358
1359 SetActState(x);
1360 for (int i=1; i<=elements.Length(); i++)
1361 {
1362 Element* e = elements(i);
1363 int is = e->IS();
1364 if (is)
1365 {
1366 if(e->IsDependent(jc))
1367 {
1368 off2=2*e->SOS()+e->ES();
1369 tempf.SetLen(is);
1370 tempf.SetAll(0);
1371 e->EvalG(tempf,t);
1372 for (int j=1; j <= is; j++)
1373 {
1374 f.Entry(rowind(e->LTG(j+off2)+off)) += tempf(j);
1375 }
1376 }
1377 }
1378 }
1379
1380 for (int i=1; i <= clearind.Length(); i++) rowind(clearind(i)) = 0;
1381
1382 //for (int i=1; i <= f.NEntries(); i++) f.Entry(i) *= -1;
1383
1384 TMStopTimer(8);
1385 }
1386
GetKineticEnergy()1387 double MultiBodySystem::GetKineticEnergy()
1388 {
1389 double en=0;
1390 for (int i=1; i<=elements.Length(); i++)
1391 {
1392 en += elements(i)->GetKineticEnergy();
1393 }
1394 return en;
1395 }
1396
GetPotentialEnergy()1397 double MultiBodySystem::GetPotentialEnergy()
1398 {
1399 double en=0;
1400 for (int i=1; i<=elements.Length(); i++)
1401 {
1402 en += elements(i)->GetPotentialEnergy();
1403 }
1404 return en;
1405 }
1406
1407
1408
GetImplicitSize() const1409 int MultiBodySystem::GetImplicitSize() const
1410 {
1411 int s=0;
1412 for (int i=1; i<=elements.Length(); i++)
1413 {
1414 s+=elements(i)->IS();
1415 }
1416 return s;
1417 }
1418
GetSecondOrderSize() const1419 int MultiBodySystem::GetSecondOrderSize() const
1420 {
1421 int s=0;
1422 for (int i=1; i<=elements.Length(); i++)
1423 {
1424 s+=elements(i)->SOSowned();
1425 }
1426 for (int i=1; i<=nodes.Length(); i++)
1427 {
1428 s+=nodes(i)->SOS();
1429 }
1430 return s;
1431 }
1432
GetSecondOrderSize_RS() const1433 int MultiBodySystem::GetSecondOrderSize_RS() const
1434 {
1435 int s=0;
1436 for (int i=1; i<=elements.Length(); i++)
1437 {
1438 s+=elements(i)->SOSowned_RS();
1439 }
1440 for (int i=1; i<=nodes.Length(); i++)
1441 {
1442 s+=nodes(i)->SOS();
1443 }
1444 return s+resortsize_add;
1445 }
1446
GetFirstOrderSize() const1447 int MultiBodySystem::GetFirstOrderSize() const
1448 {
1449 int s=0;
1450 for (int i=1; i<=elements.Length(); i++)
1451 {
1452 s+=elements(i)->ES();
1453 }
1454 return s;
1455 }
1456
GetResortSize() const1457 int MultiBodySystem::GetResortSize() const
1458 {
1459 int s=0;
1460 if (!ResortActive()) return 0;
1461
1462 for (int i=1; i<=elements.Length(); i++)
1463 {
1464 if (ResortMode2())
1465 {
1466 s += abs((elements(i)->SOSowned_RS()-elements(i)->SOSowned()));
1467 }
1468 else
1469 {
1470 s += (elements(i)->SOSowned_RS()-elements(i)->SOSowned());
1471 }
1472 }
1473 return s+resortsize_add;
1474 }
1475
GetDataSize() const1476 int MultiBodySystem::GetDataSize() const //size of data variables for each time step of system
1477 {
1478 int s=0;
1479 for (int i=1; i<=elements.Length(); i++)
1480 {
1481 s+=elements(i)->DataS();
1482 }
1483 //$ MaSch 2013-10-28: added support of SPH particle data via auxiliary elements
1484 for (int i=1; i<=auxelements.Length(); i++)
1485 {
1486 if(auxelements(i)->IsType(TParticle))
1487 {
1488 s+=auxelements(i)->DataS();
1489 }
1490 }
1491 return s;
1492 }
1493
PostNewtonStep(double t)1494 double MultiBodySystem::PostNewtonStep(double t)
1495 {
1496 if (SolverPrintsDetailedOutput())
1497 {
1498 SolverUO() << "-->PostNewtonStep " << log.TInonlinit << ":";
1499 }
1500
1501 TMStartTimer(20);
1502 double s=0;
1503 for (int i=1; i<=elements.Length(); i++)
1504 {
1505 s+=elements(i)->PostNewtonStep(t);
1506 }
1507 TMStopTimer(20);
1508
1509 if (SolverPrintsDetailedOutput())
1510 {
1511 mystr str(" err=");
1512 str += mystr(s);
1513 str += mystr(", goal=");
1514 str += mystr(DiscontinuousAccuracy());
1515 str += mystr(", t=");
1516 str += mystr(t);
1517 str += mystr("\n");
1518 SolverUO() << str;
1519
1520 if (GetOptions()->LoggingOptions()->SolverPostNewtonIterationDataVector())
1521 {
1522 SolverUO() << "Data vector = " << GetDataVector() << "\n";
1523 }
1524 }
1525
1526 return s;
1527 };
1528
StartTimeStep()1529 void MultiBodySystem::StartTimeStep()
1530 {
1531 //adjusted for IOElementDataModifier by JG 2013-17-01 and MSax
1532
1533 for (int i=1; i<=elements.Length(); i++)
1534 {
1535 //first initialize all modifier elements, because these elements modify the data of other elements!
1536 if (elements(i)->GetType() & TIOElementDataModifier) elements(i)->StartTimeStep();
1537 }
1538 for (int i=1; i<=elements.Length(); i++)
1539 {
1540 if (!(elements(i)->GetType() & TIOElementDataModifier)) elements(i)->StartTimeStep();
1541 //elements(i)->StartTimeStep();
1542 }
1543 };
1544
EndTimeStep()1545 void MultiBodySystem::EndTimeStep()
1546 {
1547 for (int i=1; i<=elements.Length(); i++)
1548 {
1549 elements(i)->EndTimeStep();
1550 }
1551 };
1552
ComputationFinished()1553 void MultiBodySystem::ComputationFinished()
1554 {
1555 for (int i=1; i<=elements.Length(); i++)
1556 {
1557 elements(i)->ComputationFinished();
1558 }
1559 };
1560
PostprocessingStep()1561 void MultiBodySystem::PostprocessingStep()
1562 {
1563 SetActState(GetSolVector());
1564 for (int i=1; i<=elements.Length(); i++)
1565 {
1566 elements(i)->PostprocessingStep();
1567 }
1568 };
1569
GetError()1570 double MultiBodySystem::GetError()
1571 {
1572 SetActState(GetSolVector());
1573 double err=0;
1574 for (int i=1; i <= elements.Length(); i++)
1575 {
1576 err+=elements(i)->GetError();
1577 }
1578 int numc = MBSss-MBSis;
1579 if (numc!=0) err = sqrt(err)/(double)numc;
1580 else err = sqrt(err);
1581
1582 return err;
1583 }
1584
GetComputationStepEDC(int computation_step_number)1585 ElementDataContainer* MultiBodySystem::GetComputationStepEDC(int computation_step_number)
1586 {
1587 ElementDataContainer* edc = GetMBS_EDC_Options();
1588 ElementDataContainer* CSEDC;
1589 ElementData* ed = edc->TreeFind("SolverOptions.ComputationSteps");
1590 if(ed && ed->IsEDC() )
1591 {
1592 CSEDC = ed->GetEDC();
1593 }
1594 else
1595 return NULL; // no EDC ComputationSteps
1596
1597 mystr step_tag = mystr("Step") + mystr(computation_step_number);
1598 ed = CSEDC->TreeFind(step_tag);
1599 if(ed && ed->IsEDC())
1600 {
1601 return ed->GetEDC();
1602 }
1603 else
1604 return NULL; // no EDC Step#
1605 }
1606
ParseStepEndTimes()1607 int MultiBodySystem::ParseStepEndTimes()
1608 {
1609 CSEndTimes.Flush();
1610
1611 // declaration & first iteration
1612 ElementDataContainer* stepEDC = GetComputationStepEDC(1);
1613 double stependtime = 0.;
1614 double laststependtime = 0.;
1615
1616 while (stepEDC)
1617 {
1618 stependtime = stepEDC->TreeGetDouble("step_end_time");
1619 if(stependtime > laststependtime) // thus all step_end_times > 0.
1620 {
1621 CSEndTimes.Add(stependtime);
1622
1623 // next iteration
1624 laststependtime = stependtime;
1625 stepEDC = GetComputationStepEDC(CSEndTimes.Length()+1);
1626 }
1627 else
1628 {
1629 UO().InstantMessageText(mystr("step_end_time of Step") + mystr(CSEndTimes.Length()+1) + mystr("is NOT higher then in previous step!\n parsing stopped, all following Computation steps are not accounted for"));
1630 break;
1631 }
1632 }
1633 // in case no computation step is defined ...
1634 if(CSEndTimes.Length() == 0)
1635 {
1636 CSEndTimes.Add(solset.endtime);
1637 }
1638
1639 return CSEndTimes.Length();
1640 }
1641
DoubleCheckWithLoadSettings()1642 int MultiBodySystem::DoubleCheckWithLoadSettings()
1643 {
1644 int nr_cs = CSEndTimes.Length();
1645 int nr_ls = GetMaxLoadSteps();
1646
1647 while (CSEndTimes.Length() < nr_ls)
1648 {
1649 AddDummyComputationStep();
1650 nr_cs = CSEndTimes.Length();
1651 }
1652 return nr_cs;
1653 }
1654
1655 // maximal number of loadsteps from all loads - there MUST be at least that many computation steps...
GetMaxLoadSteps() const1656 int MultiBodySystem::GetMaxLoadSteps() const
1657 {
1658 int max = 0;
1659
1660 for(int i=1; i <= GetNElements(); i++)
1661 {
1662 const Element& el=GetElement(i);
1663
1664 for (int j=1; j <= el.NLoads(); j++)
1665 {
1666 const MBSLoad& load = el.GetLoad(j);
1667 if(max < load.NLoadSteps())
1668 max=load.NLoadSteps();
1669 }
1670 }
1671 return max;
1672 }
1673
1674 // additional computationsteps if loadsteps supercede computation steps, interval always 1 sec*
AddDummyComputationStep()1675 int MultiBodySystem::AddDummyComputationStep()
1676 {
1677 int nr_cs = CSEndTimes.Length(); // existing computation step entries
1678 double endtime = 1;
1679 if (nr_cs) endtime = CSEndTimes.Last() + 1.; // end time
1680
1681 CSEndTimes.Add(endtime);
1682
1683 // entry in EDC - entries not temporary !!!
1684 ElementDataContainer* edc = GetMBS_EDC_Options();
1685 mystr tree = mystr("SolverOptions.ComputationSteps.Step") + mystr(nr_cs+1);
1686 ElementData ed;
1687 ed.SetDouble(endtime,"step_end_time");
1688 edc->TreeAdd(tree,ed);
1689
1690 return CSEndTimes.Length();
1691 }
1692
1693
ApplyComputationStepSettings(int stepnumber)1694 int MultiBodySystem::ApplyComputationStepSettings(int stepnumber)
1695 {
1696 UO(UO_LVL_ext) << mystr("*** Computation Step ") + mystr(stepnumber) + mystr("@ t= ") + mystr(GetTime()) + mystr(" ***\n");
1697 // get step EDC
1698 ElementDataContainer* stepEDC = GetComputationStepEDC(stepnumber);
1699
1700 if(stepEDC != NULL)
1701 {
1702 ElementDataContainer edc;
1703 // solset -> EDC
1704 SolverOptions2EDC(&edc);
1705 // EDC + changes
1706 ElementData* ed_solveroptions = edc.TreeFind("SolverOptions"); // variant a) pick subedc "SolverOptions" to write to rather then insert a level "SolverOptions" the step EDC ...
1707 if(ed_solveroptions && ed_solveroptions->IsEDC() )
1708 {
1709 //$ AD: 2011-11-08: add step_end_time manually to prevent popups
1710 ElementData* dummy = stepEDC->TreeFind("step_end_time");
1711 ed_solveroptions->GetEDC()->Add(*dummy);
1712 ed_solveroptions->GetEDC()->TreeReplaceEDCDataWith(stepEDC,1);
1713 }
1714 else
1715 return -1; // rv "-1" for "no SolverOptions found, no changes applied"
1716 // EDC -> solset
1717 EDC2SolverOptions(&edc);
1718
1719 return stepnumber;
1720 }
1721 else
1722 return 0; // rv "0" for "no EDC found, no changes applied"
1723 }
1724
1725
1726 // add sensors for all "TController"-elements
AddControllerSensors()1727 void MultiBodySystem::AddControllerSensors()
1728 {
1729 /*
1730 for(int i=1; i <= GetNElements(); i++)
1731 {
1732 const Element& el=GetElement(i);
1733
1734 if(el.IsType(TController))
1735 {
1736 UO() << "elem" << i << ", no=" << ((InputOutputElement&)el).GetNOutputs() << "\n";
1737 for(int j=1; j <= ((InputOutputElement&)el).GetNOutputs(); j++)
1738 {
1739 MBSSensor s(this, TMBSSensor(TSOutputSensor), i, j);
1740 s.SetSensorName(mystr("ControllerElement") + mystr(i) + mystr("_output") + mystr(j));
1741 AddSensor(&s);
1742 UO() << "Add controller sensor (elementNum = " << i << ", outputNum = " << j << "\n";
1743 }
1744 }
1745 }
1746 */
1747 }
1748
1749 //$ YV 2011-06-07:[ placed this piece of code into a separate function
BuildLTGLists()1750 void MultiBodySystem::BuildLTGLists()
1751 {
1752 //reset nodes and elements LTG, for multiple call of assemble!
1753 for (int i=1; i<=nodes.Length(); i++)
1754 {
1755 GetNode(i).LTGreset();
1756 GetNode(i).ResetNodeToElementList();
1757 }
1758
1759 int warn1 = 0;
1760 for (int i=1; i<=elements.Length(); i++)
1761 {
1762 //build node_to_element lists in nodes
1763 for (int j=1; j<=GetElement(i).NNodes(); j++)
1764 {
1765 //if (!GetElement(i).GetNode(j).IsAuxNode())
1766 GetElement(i).GetNode(j).AddElementNumber(i);
1767 }
1768 //initialize LTG lists
1769 /*if (GetElement(i).IsType(TCMSflag) && !warn1)
1770 {
1771 warn1 = 1;
1772 UO() << "Assemble: check if still works with CMS elements!!!\n";
1773 }*/
1774 GetElement(i).LTGreset();
1775 GetElement(i).RemoveConstraints();
1776 GetElement(i).LTGdataReset();
1777 }
1778
1779 for (int i=1; i<=auxelements.Length(); i++)
1780 {
1781 for (int j=1; j<=GetAuxElement(i).NNodes(); j++)
1782 {
1783 //if (!GetElement(i).GetNode(j).IsAuxNode())
1784 GetAuxElement(i).GetNode(j).AddElementNumber(i);
1785 }
1786 }
1787
1788 //These numbers are offsets!!!!!
1789 int sos1 = 1;
1790 int sos2 = GetSecondOrderSize()+1;
1791 int es = GetSecondOrderSize()*2+1;
1792 int is = GetSecondOrderSize()*2+GetFirstOrderSize()+1;
1793 int datas = 1;
1794
1795 //DOF-references
1796 for (int i=1; i<=nodes.Length(); i++)
1797 {
1798 for (int j=1; j <= GetNode(i).SOS(); j++)
1799 {
1800 GetNode(i).AddLTG(sos1++);
1801 }
1802 for (int j=1; j <= GetNode(i).SOS(); j++)
1803 {
1804 GetNode(i).AddLTG(sos2++);
1805 }
1806 }
1807
1808 for (int i=1; i<=elements.Length(); i++)
1809 {
1810 //do not clear ltg-lists, because elements might have already built up lists (CMS-element)??
1811 Element* e = elements(i);
1812 for (int j=1; j <= e->SOSowned(); j++)
1813 {
1814 e->AddLTG(sos1++);
1815 }
1816 for (int j=1; j <= e->SOSowned(); j++)
1817 {
1818 e->AddLTG(sos2++);
1819 }
1820 for (int j=1; j <= e->ES(); j++)
1821 {
1822 e->AddLTG(es++);
1823 }
1824 for (int j=1; j <= e->IS(); j++)
1825 {
1826 e->AddLTG(is++);
1827 }
1828 for (int j=1; j <= e->DataS(); j++)
1829 {
1830 e->AddLTGdata(datas++);
1831 }
1832 e->LinkToElements();
1833
1834 e->LinkLoads();
1835 //uo << "ltg" << i << "=" << e->LTG(1) << ", " << e->LTG(2) << "\n";
1836 }
1837
1838 //$ MaSch 2013-10-28: added support of SPH particle data via auxiliary elements
1839 for (int i=1; i<=auxelements.Length(); i++)
1840 {
1841 Element* e = auxelements(i);
1842 if(e->IsType(TParticle))
1843 {
1844 for (int j=1; j <= e->DataS(); j++)
1845 {
1846 e->AddLTGdata(datas++);
1847 }
1848 }
1849 }
1850
1851
1852 }
1853 //$ YV 2011-06-07:]
1854
1855 //create LTG lists:
Assemble()1856 void MultiBodySystem::Assemble()
1857 {
1858 int info = 0;
1859 if (info) UO() << "start assemble\n";
1860
1861 for (int i=1; i<=elements.Length() + auxelements.Length(); i++)
1862 {
1863 Element * e;
1864 if(i <= elements.Length())
1865 e = &GetElement(i);
1866 else
1867 e = &GetAuxElement(i - elements.Length());
1868 e->PreAssemble();
1869 }
1870
1871 BuildLTGLists();
1872
1873 if (UseDependencies())
1874 {
1875 for (int i=1; i<=elements.Length(); i++)
1876 {
1877 Element* e = elements(i);
1878 e->BuildDependencies();
1879 }
1880 }
1881 else
1882 {
1883 // uo << "**************\nWARNING: dependencies not built!!!\n**************\n";
1884 }
1885
1886 LabelIOElementOutputs(); //
1887 FilterIOElements(); // create a shortlist of all IO elements in the MBS
1888
1889 //$ YV 2011-06-07:[ the following line is commented out, as resorting is not needed any more
1890 //ComputeResortVector();
1891
1892 //set outer faces for drawing
1893 //$ YV 2012-12-13: setting outer faces for drawing has been moved to FiniteElement3D::PreAssemble()
1894
1895 CollectAvailableFieldVariables();
1896 if (info)
1897 UO() << "finished assembling\n";
1898 }
1899
1900 //$ AD: new 2013-02-01 see Log 388
LabelIOElementOutputs()1901 void MultiBodySystem::LabelIOElementOutputs()
1902 {
1903 // output to Load
1904 for (int i=1; i<=NLoads(); i++)
1905 {
1906 if (GetLoad(i).HasIOElement())
1907 {
1908 int elemnr = -1;
1909 int outputnr = -1;
1910 GetLoad(i).GetIOElementNr(elemnr, outputnr);
1911 if (elemnr > 0 && outputnr > 0)
1912 {
1913 if (elemnr <= NE())
1914 {
1915 if (GetElement(elemnr).IsType(TMBSElement(TController+TConstraint)))
1916 {
1917 //InputOutputElement* ioelem = (InputOutputElement*)GetElementPtr(elemnr); // does not agree with "interface idea" ...
1918 Element* ioelem = GetElementPtr(elemnr);
1919
1920 if (outputnr <= ioelem->GetNOutputs())
1921 {
1922 mystr str = mystr("L") + mystr(i) + mystr(": ") + GetLoad(i).LoadName();
1923 // mystr str = mystr("L") + mystr(i) + mystr(": ") + GetLoad(i).GetLoadTypeString();
1924 ioelem->SetOutputName(outputnr, str);
1925 }
1926 else
1927 UO() << mystr("ERROR: Load ") + mystr(i) + mystr(": referenced output does not exist on element. \n");
1928 }
1929 else
1930 UO() << mystr("ERROR: Load ") + mystr(i) + mystr(": referenced element is not an IOElement. \n");
1931 }
1932 else
1933 UO() << mystr("ERROR: Load ") + mystr(i) + mystr(" has an illegal entry for IOElement number. \n");
1934 }
1935 else
1936 UO() << mystr("ERROR: Load ") + mystr(i) + mystr(" has an illegal entry for IOElement number and/or output number. \n");
1937 }
1938 }
1939 // Sensor
1940 }
1941
FilterIOElements()1942 int MultiBodySystem::FilterIOElements()
1943 {
1944 ioelements.Flush();
1945 for (int i=1; i<=NE(); i++)
1946 {
1947 if (GetElement(i).IsType(TMBSElement(TController+TConstraint)))
1948 {
1949 ioelements.Add(i);
1950 }
1951 }
1952 return ioelements.Length();
1953 }
1954
RespondToKey(int key)1955 int MultiBodySystem::RespondToKey(int key)
1956 {
1957 int nr_changes = 0;
1958 for (int i=1; i<=ioelements.Length(); i++)
1959 {
1960 nr_changes += GetElement(ioelements(i)).RespondToKey(key);
1961 }
1962 return nr_changes;
1963 }
1964
1965 //void MultiBodySystem::RespondToMouse(double x, double y)
1966 //{
1967 //
1968 //}
1969
1970
1971
1972 //Vector for resorting the band mass- and stiffness matrix such that it is regular
1973 //-> for quaternion elements, the constraints are resorted to the SOS part!!!
ComputeResortVector()1974 void MultiBodySystem::ComputeResortVector()
1975 {
1976 //These numbers are offsets!!!!!
1977 int rs = GetResortSize();
1978 int sos = GetSecondOrderSize();
1979 int sos_rs = GetSecondOrderSize_RS();
1980 int es = GetFirstOrderSize();
1981 int is = GetImplicitSize();
1982 int ne = elements.Length();
1983
1984 //the resorted vector has only components sos+es+is, not 2*sos;
1985 //therefore, the sorting subtracts sos for es and is components!!!!
1986
1987 //UO() << "Re-sort size=" << rs << "\n";
1988
1989 resortsize_add = 0;
1990
1991 IVector resortconstraint;
1992 resortconstraint.SetLen(ne);
1993 for (int i = 1; i <= ne; i++)
1994 resortconstraint(i) = 0;
1995
1996 resortvector.SetLen(0);
1997
1998 int actdofind = 1;
1999 int actnode = 1;
2000
2001 for (int i=1; i<=ne; i++)
2002 {
2003 Element* e = elements(i);
2004
2005 //UO() << "El" << i << ", SOS2=" << e->SOSowned() << ", SOS2_RS=" << e->SOSowned_RS() << "\n";
2006
2007 //DOF-references
2008 for (int k=actnode; k<=nodes.Length(); k++)
2009 {
2010 actnode = k;
2011 int nsos = GetNode(k).SOS();
2012 for (int j=1; j <= nsos; j++)
2013 {
2014 resortvector.Add(GetNode(k).Get(j));
2015 }
2016 if ((nsos && GetNode(k).Get(nsos) > actdofind) && !(i == ne)) break;
2017 }
2018 actnode++;
2019
2020
2021 if (ResortMode2() && (e->SOSowned() > e->SOSowned_RS()))
2022 {
2023 //fill in part which goes to sos2 part
2024 for (int j=1; j <= e->SOSowned_RS(); j++)
2025 {
2026 resortvector.Add(e->LTG(j));
2027 }
2028 //other part is filled together with constraints
2029 }
2030 else //old mode, can only put constraints into sos-part
2031 {
2032 for (int j=1; j <= e->SOSowned(); j++)
2033 {
2034 resortvector.Add(e->LTG(j));
2035 }
2036 for (int j=e->SOSowned()+1; j <= e->SOSowned_RS(); j++)
2037 {
2038 resortvector.Add(e->LTG(j+e->SOSowned()+e->ES())-sos);
2039 }
2040 }
2041 if (resortvector.Length() != 0) actdofind = resortvector.Last();
2042
2043 if (!TransformJacApply()) //sort constraints into multibody system -> better band structure
2044 {
2045 for (int k=1; k <= Minimum(e->NC(),2); k++) //cccc
2046 {
2047 //UO() << "constraint " << k << "\n";
2048 //$ YV 2012-12-13: we can be sure that Constraint is derived from Element
2049 Element * c = (Element*)(e->GetConstraint(k));
2050 int cind = c->GetOwnNum();
2051
2052 int dist = 0;
2053 const int maxdist = 2; //cccc
2054 if (c->NE() == 2)
2055 {
2056 dist = abs(c->GetElnum(1)-c->GetElnum(2));
2057 }
2058 else if (c->NE() == 1)
2059 {
2060 dist = 1;
2061 }
2062 else
2063 {
2064 dist = maxdist+1; //do not resort
2065 }
2066
2067 //UO() << "c-NE=" << c->NE() << "\n";
2068
2069 if (c->ES() > 0 || c->SOS() > 0)
2070 {
2071 dist = maxdist+1; // do not resort if special actuators, etc.
2072 }
2073 //UO() << "constraint" << cind << ": dist=" << dist << "\n";
2074
2075 if (!resortconstraint(cind) && dist <= maxdist)
2076 {
2077 resortconstraint(cind) = 1;
2078 for (int j=1; j <= c->ES(); j++)
2079 {
2080 resortvector.Add(c->LTG(c->SOSowned_RS()*2+j)-sos);
2081 resortsize_add++;
2082 }
2083
2084 for (int j=1; j <= c->IS_RS(); j++)
2085 {
2086 resortvector.Add(c->LTG(c->SOSowned_RS()*2+c->ES()+j)-sos);
2087 resortsize_add++;
2088 }
2089 }
2090 }
2091 }
2092 }
2093 for (int i=1; i<=ne; i++)
2094 {
2095 Element* e = elements(i);
2096 if (!resortconstraint(i))
2097 for (int j=1; j <= e->ES(); j++)
2098 {
2099 resortvector.Add(e->LTG(e->SOSowned_RS()*2+j)-sos);
2100 }
2101 }
2102
2103 /* //not necessary, everything is done with IS_RS()
2104 if (ResortMode2() && 0)
2105 {
2106 //add part which goes to constraint part:
2107 for (int i=1; i<=ne; i++)
2108 {
2109 Element* e = elements(i);
2110 if (!resortconstraint(i) && (e->SOSowned() > e->SOSowned_RS()))
2111 {
2112 for (int j=e->SOSowned_RS()+1; j <= e->SOSowned(); j++)
2113 {
2114 resortvector.Add(e->LTG(j));
2115 }
2116 }
2117 }
2118 }*/
2119
2120 for (int i=1; i<=ne; i++)
2121 {
2122 Element* e = elements(i);
2123 if (!resortconstraint(i))
2124 for (int j=1; j <= e->IS_RS(); j++)
2125 {
2126 resortvector.Add(e->LTG(e->SOSowned()+e->SOSowned_RS()+e->ES()+j)-sos); //changed e->SOSowned_RS()*2 to e->SOSowned_RS()+e->SOSowned() .. not tested
2127 }
2128 }
2129
2130 if (!ResortActive())
2131 {
2132 //UO() << "Resort deactivated!!!\n";
2133 for (int i=1; i <= sos+es+is; i++)
2134 resortvector(i) = i;
2135 }
2136
2137 /*
2138 UO() << "Re-sort size new=" << GetResortSize() << "\n";
2139
2140 UO() << "resortvector=[";
2141 for (int i=1; i <= resortvector.Length(); i++)
2142 UO() << resortvector(i) << ",";
2143 UO() << "]\n";
2144
2145 UO() << "resortconstraint=[";
2146 for (int i=1; i <= resortconstraint.Length(); i++)
2147 UO() << resortconstraint(i) << ",";
2148 UO() << "]\n";
2149
2150 //UO().InstantMessageText("Resortvector");
2151 */
2152
2153 }
2154
ComputeMaxSparseBandwidth()2155 void MultiBodySystem::ComputeMaxSparseBandwidth()
2156 {
2157 elementbandwidth = 4; //minimum
2158 for (int i=1; i<=elements.Length(); i++)
2159 {
2160 Element* e = elements(i);
2161 elementbandwidth = Maximum(elementbandwidth,e->ElementBandwidth());
2162 }
2163 //UO() << "ComputeMaxSparseBandwidth=" << elementbandwidth << "\n";
2164 }
2165
2166
TransformJacApply() const2167 int MultiBodySystem::TransformJacApply() const
2168 {
2169 return transformJacApply;
2170 }
2171
ApplyTransform(const Vector & v,Vector & Av,int mode)2172 void MultiBodySystem::ApplyTransform(const Vector& v, Vector& Av, int mode) //compute Av=A^T*v in mode==0 and Av=A*v in mode==1
2173 {
2174 //should only be called by NumNLSys if transformJacApply is set
2175 Av = v;
2176 for (int i=1; i<=elements.Length(); i++)
2177 {
2178 if (elements(i)->TransformJacApply())
2179 elements(i)->ApplyTransform(v, Av, mode);
2180 }
2181 }
2182
2183 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2184 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2185
AddElement(Element * e)2186 int MultiBodySystem::AddElement(Element* e)
2187 {
2188 Element* ec = e->GetCopy();
2189 //ec->GetCol()=ec->GetMaterial().GetMaterialColor(); // element gets color from material:
2190 elements.Add(ec);
2191 elements.Last()->SetOwnNum(elements.Length());
2192
2193 return elements.Length();
2194 }
2195
2196 //$ DR 2013-05-23 removed deprecated function AddBody
2197 //int MultiBodySystem::AddBody(Element* e)
2198 //{
2199 // //search bodies:
2200 // int found = 0;
2201 // int i = 1;
2202 // while (i <= NE() && !found)
2203 // {
2204 // if (!GetElement(i).IsType(TBody)) found = i;
2205 // i++;
2206 // }
2207 //
2208 // if (found)
2209 // {
2210 // InsertElement(found, e);
2211 // return found; //must return element number!
2212 // }
2213 // else
2214 // {
2215 // Element* ec = e->GetCopy();
2216 // elements.Add(ec);
2217 // elements.Last()->SetOwnNum(elements.Length());
2218 // return elements.Length();
2219 // }
2220 //
2221 //}
2222
2223 //$ DR 2013-05-23 removed deprecated function InsertElement
2224 //void MultiBodySystem::InsertElement(int i, Element* e)
2225 //{
2226 // //resort element list:
2227 // elements.SetLen(NE()+1);
2228 //
2229 // for (int j=NE(); j > i; j--)
2230 // {
2231 // elements(j) = elements(j-1);
2232 // }
2233 //
2234 // Element* ec = e->GetCopy();
2235 // elements(i) = ec;
2236 //
2237 // //set new element numbers:
2238 // for (int j=i; j <= NE(); j++)
2239 // {
2240 // elements(j)->SetOwnNum(j);
2241 // }
2242 //
2243 // //update element numbers of constraints:
2244 // for (int j=1; j <= NE(); j++)
2245 // {
2246 // if (elements(j)->IsType(TConstraint))
2247 // {
2248 // //$ YV 2012-12-13: Constraint is just an Element here
2249 // Element* c = elements(j);
2250 // for (int k=1; k <= c->NE(); k++)
2251 // {
2252 // if (c->GetElnum(k) >= i)
2253 // {
2254 // c->SetElnum(k, c->GetElnum(k)+1);
2255 // }
2256 // }
2257 // }
2258 // }
2259 // for (int j=NSensors(); j >= 1; j--)
2260 // {
2261 // for (int k=1; k <= sensors(j)->GetNumberOfRelatedElements(); k++)
2262 // {
2263 // if (sensors(j)->GetRelatedElementNumber(k) >= i)
2264 // {
2265 // mystr tname = sensors(j)->GetTypeName();
2266 // sensors(j)->GetRelatedElementNumber(k)++;
2267 // //set new sensor names with new element numbers ...
2268 // if (sensors(j)->GetSensorName() == tname)
2269 // {
2270 // sensors(j)->SetSensorName(sensors(j)->GetTypeName());
2271 // }
2272 // }
2273 // }
2274 // }
2275 //}
2276
2277
AddAuxElement(Element * e)2278 int MultiBodySystem::AddAuxElement(Element* e)
2279 {
2280 Element* ec = e->GetCopy();
2281 auxelements.Add(ec);
2282 //UO() << "add aux-elem=" << auxelements.Length() << "\n";
2283
2284 return auxelements.Length();
2285 }
2286
AddNode(Node * n)2287 int MultiBodySystem::AddNode(Node* n)
2288 {
2289 Node* nc = n->GetCopy();
2290 nc->SetMBS(this);
2291 int j = nodes.Add(nc);
2292 nc->NodeNum() = j;
2293 return j;
2294 }
2295
AddNode(Node * n,SearchTree & tree)2296 int MultiBodySystem::AddNode(Node* n, SearchTree& tree)
2297 {
2298 //only works, if searchtree contains all existing nodes!
2299
2300 double tol = 1e-8*CharacteristicLength();
2301
2302 IVector items;
2303
2304 Box3D box(n->Pos(),n->Pos());
2305 box.Increase(tol);
2306
2307 tree.GetItemsInBox(box,items);
2308 for (int i = 1; i <= items.Length(); i++)
2309 {
2310 if (Dist(n->Pos(),nodes(items(i))->Pos()) <= tol) return items(i);
2311 }
2312 Node* nc = n->GetCopy();
2313 nc->SetMBS(this);
2314
2315 int index = nodes.Add(nc);
2316 nc->NodeNum() = index;
2317 tree.AddItem(Box3D(n->Pos(),n->Pos()), index);
2318
2319 return index;
2320 }
2321
AddBodyNode(Node * n)2322 int MultiBodySystem::AddBodyNode(Node* n)
2323 {
2324 for (int i = 1; i <=nodes.Length(); i++)
2325 {
2326 if (Dist(n->Pos(),nodes(i)->Pos()) <= 1e-8*CharacteristicLength() && n->GetBodyInd() == nodes(i)->GetBodyInd())
2327 {
2328 return i;
2329 }
2330 }
2331 Node* nc = n->GetCopy();
2332 nc->SetMBS(this);
2333 int j = nodes.Add(nc);
2334 nc->NodeNum() = j;
2335 return j;
2336 }
2337
AddBodyNode(Node * n,SearchTree & tree)2338 int MultiBodySystem::AddBodyNode(Node* n, SearchTree& tree)
2339 {
2340 //$ AD 2011-02: AddBodyNode with Searchtree
2341
2342 double tol = 1e-8*CharacteristicLength();
2343
2344 IVector items;
2345
2346 Box3D box(n->Pos(),n->Pos());
2347 box.Increase(tol);
2348
2349 tree.GetItemsInBox(box,items);
2350 for (int i = 1; i <= items.Length(); i++)
2351 {
2352 if ( (Dist(n->Pos(),nodes(items(i))->Pos()) <= tol) && (n->GetBodyInd() == nodes(items(i))->GetBodyInd()) ) return items(i);
2353 }
2354 Node* nc = n->GetCopy();
2355 nc->SetMBS(this);
2356
2357 int index = nodes.Add(nc);
2358 nc->NodeNum() = index;
2359 tree.AddItem(Box3D(n->Pos(),n->Pos()), index);
2360
2361 return index;
2362 }
2363
2364
2365
AddSensor(Sensor * s)2366 int MultiBodySystem::AddSensor(Sensor* s)
2367 {
2368 Sensor* sc = s->GetCopy();
2369 sensors.Add(sc);
2370 sensors.Last()->SetOwnNum(sensors.Length());
2371
2372 return sensors.Length();
2373 }
2374
2375 //$ DR 2012-10: loads moved from element to mbs
AddLoad(const MBSLoad & li)2376 int MultiBodySystem::AddLoad(const MBSLoad& li)
2377 {
2378 //$ YV 2013-01-03: here we use GetCopy instead
2379 //MBSLoad* l = new MBSLoad(li);
2380 MBSLoad* l = li.GetCopy();
2381 l->SetOwnNum(loads.Length()+1);
2382 return loads.Add(l);
2383
2384 // the following code is still in element.h/cpp:
2385
2386 //int elem = li.HasElementDependence();
2387 //if (elem)
2388 //{
2389 // TArray<int> elnums;
2390
2391 // elnums.Add(elem);
2392 // GetElement(elem).GetDirectFeedThroughElements(elnums);
2393
2394 // for (int i=1; i<=elnums.Length(); i++)
2395 // {
2396 // elements.Add(elnums(i));
2397 // }
2398 //}
2399 }
2400
2401 //void Element::AddLoad(const MBSLoad& li)
2402 //{
2403 // MBSLoad* l = new MBSLoad(li);
2404 // loads.Add(l);
2405 //
2406 // int elem = li.HasElementDependence();
2407 // if (elem)
2408 // {
2409 // TArray<int> elnums;
2410 //
2411 // elnums.Add(elem);
2412 // GetMBS()->GetElement(elem).GetDirectFeedThroughElements(elnums);
2413 // //UO() << "Elem" << GetOwnNum() << ": Add load dependence elements: " << elnums << "\n";
2414 //
2415 // for (int i=1; i<=elnums.Length(); i++)
2416 // {
2417 // elements.Add(elnums(i));
2418 // }
2419 // }
2420 //}
2421
AddRigidBodyDOFSensor(int bodynum,const Vector3D & pos_rel,mystr general_str)2422 void MultiBodySystem::AddRigidBodyDOFSensor(int bodynum, const Vector3D& pos_rel, mystr general_str)
2423 {
2424 /*
2425 MBSSensor sensAPosX(this, (TMBSSensor)(TSElement+TSPos+TSX), bodynum, pos_rel);
2426 sensAPosX.SetSensorName(general_str + mystr("_pos_x"));
2427 AddSensor(&sensAPosX);
2428 MBSSensor sensAPosY(this, (TMBSSensor)(TSElement+TSPos+TSY), bodynum, pos_rel);
2429 sensAPosY.SetSensorName(general_str + mystr("_pos_y"));
2430 AddSensor(&sensAPosY);
2431 MBSSensor sensAPosZ(this, (TMBSSensor)(TSElement+TSPos+TSZ), bodynum, pos_rel);
2432 sensAPosZ.SetSensorName(general_str + mystr("_pos_z"));
2433 AddSensor(&sensAPosZ);
2434
2435 MBSSensor sensAngX(this, (TMBSSensor)(TSElement+TSAngle), bodynum, pos_rel, Vector3D(1.,0.,0.), Vector3D(0.,0.,1.), Vector3D(0.,0.,1.));
2436 sensAngX.SetSensorName(general_str + mystr("_glob_ang_x"));
2437 AddSensor(&sensAngX);
2438 MBSSensor sensAngY(this, (TMBSSensor)(TSElement+TSAngle), bodynum, pos_rel, Vector3D(0.,1.,0.), Vector3D(0.,0.,1.), Vector3D(0.,0.,1.));
2439 sensAngY.SetSensorName(general_str + mystr("_glob_ang_y"));
2440 AddSensor(&sensAngY);
2441 MBSSensor sensAngZ(this, (TMBSSensor)(TSElement+TSAngle), bodynum, pos_rel, Vector3D(0.,0.,1.), Vector3D(1.,0.,0.), Vector3D(1.,0.,0.));
2442 sensAngZ.SetSensorName(general_str + mystr("_glob_ang_z"));
2443 AddSensor(&sensAngZ);
2444 */
2445 }
2446
2447 // old version (AD)
2448 //void MultiBodySystem::AddRigid2DBodyDOFSensor(int bodynum, mystr general_str, int usepos, int usevel, int displacement) // old version
2449 //{
2450 // AddRigid2DBodyDOFSensor(bodynum,Vector2D(0.0,0.0),general_str,usepos,usevel,displacement);
2451 //}
2452
2453 // new version (AD), please change code accordingly (insert Vector2D& pos_rel)
AddRigid2DBodyDOFSensor(int bodynum,Vector2D & pos_rel,mystr general_str,int usepos,int usevel,int displacement)2454 void MultiBodySystem::AddRigid2DBodyDOFSensor(int bodynum, Vector2D& pos_rel, mystr general_str, int usepos, int usevel, int displacement)
2455 {
2456 /*
2457 if(usepos)
2458 {
2459 MBSSensor sensAPosX(this, TMBSSensor(TSElement + TSPos + TSX + TSplanar), bodynum, pos_rel);
2460 sensAPosX.SetSensorName(general_str + mystr("_pos_x"));
2461 if (displacement)
2462 {
2463 sensAPosX.SetOffset(-GetElement(bodynum).GetXInit().Get(1)); // quick solution, replace with GetPosInit()...
2464 sensAPosX.SetSensorName(general_str + mystr("_disp_x"));
2465 }
2466 AddSensor(&sensAPosX);
2467
2468 MBSSensor sensAPosY(this, TMBSSensor(TSElement + TSPos + TSY + TSplanar), bodynum, pos_rel);
2469 sensAPosY.SetSensorName(general_str + mystr("_pos_y"));
2470 if (displacement)
2471 {
2472 sensAPosY.SetOffset(-GetElement(bodynum).GetXInit().Get(2)); // quick solution, replace with GetPosInit()...
2473 sensAPosY.SetSensorName(general_str + mystr("_disp_y"));
2474 }
2475 AddSensor(&sensAPosY);
2476
2477 MBSSensor sensAngX(this, TMBSSensor(TSElement + TSAngle + TSplanar), bodynum, pos_rel);
2478 sensAngX.SetSensorName(general_str + mystr("_ang_phi"));
2479 AddSensor(&sensAngX);
2480 }
2481 if(usevel)
2482 {
2483 MBSSensor sensAVelX(this, TMBSSensor(TSElement + TSVel + TSX + TSplanar), bodynum, pos_rel);
2484 sensAVelX.SetSensorName(general_str + mystr("_vel_x"));
2485 AddSensor(&sensAVelX);
2486
2487 MBSSensor sensAVelY(this, TMBSSensor(TSElement + TSVel + TSY + TSplanar), bodynum, pos_rel);
2488 sensAVelY.SetSensorName(general_str + mystr("_vel_y"));
2489 AddSensor(&sensAVelY);
2490
2491 MBSSensor sensAngVelX(this, TMBSSensor(TSElement + TSVel + TSAngle + TSplanar), bodynum, pos_rel);
2492 sensAngVelX.SetSensorName(general_str + mystr("_ang_vel_phi"));
2493 AddSensor(&sensAngVelX);
2494 }
2495 */
2496 }
2497
AddMaterial(const Material & m)2498 int MultiBodySystem::AddMaterial(const Material& m)
2499 {
2500 Material* mc = m.GetCopy();
2501
2502 materials.Add(mc);
2503 materials.Last()->SetMaterialNumber(materials.Length());
2504
2505 return materials.Length();
2506 }
2507
2508 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2509 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2510
DeleteElement(int i)2511 void MultiBodySystem::DeleteElement(int i)
2512 {
2513 ///////////----------- CHECK ---------------------------//////////////////
2514 // check if element is used by a Geomelement, not necessary, just set to 0
2515 // check if element is used by a sensor
2516 for (int j = 1; j <= NSensors(); j++)
2517 {
2518 for (int k = 1; k<=GetSensor(j).GetNumberOfRelatedElements(); k++)
2519 {
2520 if(GetSensor(j).GetRelatedElementNumber(k) == i)
2521 {
2522 UO(UO_LVL_err) << "ERROR: Can not delete element " << i << ", because it is still used by sensor " << j << "!\n";
2523 return;
2524 }
2525 }
2526 }
2527 // check if element is used by a load, not necessary, will be adjusted to non-dependent load
2528
2529 // check if element is used by an other element
2530 for (int j = 1; j <= NE(); j++)
2531 {
2532 if(j != i) // maybe some IO-Elements have themselves as inputs somewhen, then they should also be deleted
2533 {
2534 for (int k = 1; k<=GetElement(j).NE(); k++)
2535 {
2536 if(GetElement(j).GetElnum(k) == i)
2537 {
2538 UO(UO_LVL_err) << "ERROR: Can not delete element " << i << ", because it is still used by element " << j << "!\n";
2539 return;
2540 }
2541 }
2542 }
2543 }
2544
2545 ///////////----------- DELETE ---------------------------//////////////////
2546 // delete the element in the MBS
2547 delete elements(i); // just deletes the element, does not change the length of the TArray
2548 elements.Erase(i); // decreases the length of the TArray
2549 UO(UO_LVL_warn) << "Element "<< i << "deleted\n";
2550
2551 ///////////----------- ADJUST ---------------------------//////////////////
2552 //set new element numbers:
2553 for (int j=i; j <= NE(); j++)
2554 {
2555 elements(j)->SetOwnNum(j);
2556 }
2557
2558 // adjust for GeomElements
2559 for (int j = 1; j <= NDrawElements(); j++)
2560 {
2561 int num = GetDrawElement(j)->GetElnum();
2562 if (num == i)
2563 {
2564 GetDrawElement(j)->SetElnum(0);
2565 UO(UO_LVL_warn) << "WARNING: Deleted element " << i << " had been used by GeomElement " << j << ". Check the properties of the GeomElement!\n";
2566 }
2567 else if (num > i)
2568 {
2569 GetDrawElement(j)->SetElnum(num-1);
2570 }
2571 }
2572
2573 // adjust for sensors
2574 for (int j = 1; j <= NSensors(); j++)
2575 {
2576 for (int k = 1; k<=GetSensor(j).GetNumberOfRelatedElements(); k++)
2577 {
2578 if(GetSensor(j).GetRelatedElementNumber(k) > i)
2579 {
2580 GetSensor(j).GetRelatedElementNumber(k)--;
2581 }
2582 }
2583 }
2584
2585 // adjust for loads
2586 for (int j = 1; j <= NLoads(); j++)
2587 {
2588 if(GetLoad(j).HasElementDependence())
2589 {
2590 int num, output;
2591 GetLoad(j).GetIOElementNr(num, output);
2592 if (num == i)
2593 {
2594 GetLoad(j).SetLoadFunc(0); // no dependence on element i anymore
2595 UO(UO_LVL_warn) << "WARNING: Deleted element " << i << " had been used by load " << j << ". Check the properties of the load!\n";
2596 }
2597 else if (num > i)
2598 {
2599 GetLoad(j).SetIOElement(num-1, output); // adjust number
2600 }
2601 }
2602 }
2603
2604 // adjust for elements
2605 for (int j = 1; j <= NE(); j++)
2606 {
2607 for (int k = 1; k<=GetElement(j).NE(); k++)
2608 {
2609 int num = GetElement(j).GetElnum(k);
2610 if (num > i)
2611 {
2612 GetElement(j).SetElnum(k,num-1);
2613 }
2614 }
2615 }
2616 }
2617
DeleteSensor(int i)2618 void MultiBodySystem::DeleteSensor(int i)
2619 {
2620 ///////////----------- CHECK ---------------------------//////////////////
2621 // check if sensor is used by an element
2622 for (int j = 1; j <= NE(); j++)
2623 {
2624 for (int k = 1; k<=GetElement(j).NSensors(); k++)
2625 {
2626 if(GetElement(j).GetSensorNum(k) == i)
2627 {
2628 UO(UO_LVL_err) << "ERROR: Can not delete sensor " << i << ", because it is still used by element " << j << "!\n If this is a control element, just remove the sensor from the IOBlock inputs, than you can delete the sensor.\n";
2629 return;
2630 }
2631 }
2632 }
2633 // check if sensor is used by another sensor
2634 for (int j = 1; j <= NSensors(); j++)
2635 {
2636 for (int k = 1; k<=GetSensor(j).GetNumberOfRelatedSensors(); k++)
2637 {
2638 if(GetSensor(j).GetRelatedSensorNumber(k) == i)
2639 {
2640 UO(UO_LVL_err) << "ERROR: Can not delete sensor " << i << ", because it is still used by sensor " << j << "!\n";
2641 return;
2642 }
2643 }
2644 }
2645 ///////////----------- DELETE ---------------------------//////////////////
2646 // delete the sensor in the MBS
2647 delete sensors(i); // just deletes the sensor, does not change the length of the TArray
2648 sensors.Erase(i); // decreases the length of the TArray
2649 UO(UO_LVL_warn) << "Sensor "<< i << "deleted\n";
2650
2651 ///////////----------- ADJUST ---------------------------//////////////////
2652 //set new sensor numbers:
2653 for (int j=i; j <= NSensors(); j++)
2654 {
2655 sensors(j)->SetOwnNum(j);
2656 }
2657
2658 // adjust for sensors
2659 for (int j = 1; j <= NSensors(); j++)
2660 {
2661 for (int k = 1; k<=GetSensor(j).GetNumberOfRelatedSensors(); k++)
2662 {
2663 if(GetSensor(j).GetRelatedSensorNumber(k) > i)
2664 {
2665 GetSensor(j).GetRelatedSensorNumber(k)--;
2666 }
2667 }
2668 }
2669
2670 // adjust for elements
2671 for (int j = 1; j <= NE(); j++)
2672 {
2673 for (int k = 1; k<=GetElement(j).NSensors(); k++)
2674 {
2675 int num = GetElement(j).GetSensorNum(k);
2676 if (num > i)
2677 {
2678 GetElement(j).SetSensorNum(k,num-1);
2679 }
2680 }
2681 }
2682
2683
2684 }
2685
2686 //$ DR 2012-10: loads moved from element to mbs
DeleteLoad(int i)2687 void MultiBodySystem::DeleteLoad(int i)
2688 {
2689 ///////////----------- CHECK ---------------------------//////////////////
2690 // check if load is used by a sensor
2691 for (int j = 1; j <= NSensors(); j++)
2692 {
2693 for (int k = 1; k<=GetSensor(j).GetNumberOfRelatedLoads(); k++)
2694 {
2695 if(GetSensor(j).GetRelatedLoadNumber(k) == i)
2696 {
2697 UO(UO_LVL_err) << "ERROR: Can not delete load " << i << ", because it is still used by sensor " << j << "!\n";
2698 return;
2699 }
2700 }
2701 }
2702
2703 // not necessary to check for elements. if element uses load, than load is removed from this element!
2704
2705 ///////////----------- DELETE ---------------------------//////////////////
2706 // delete the load in the MBS
2707 delete loads(i); // just deletes the MBSLoad, does not change the length of the TArray
2708 loads.Erase(i); // decreases the length of the TArray
2709 UO(UO_LVL_warn) << "Load "<< i << "deleted\n";
2710
2711 // correct the entries in the elements
2712 Element* elP;
2713 TArray<int> el_loads;
2714 int elHadLoad;
2715
2716 for (int j=1; j < NE(); j++)
2717 {
2718 elP=GetElementPtr(j);
2719 el_loads.Flush();
2720 elHadLoad = 0;
2721 el_loads = elP->GetLoadNrs();
2722 for(int l=1; l<=el_loads.Length(); l++)
2723 {
2724 // find and delete the load in the element
2725 if(el_loads(l)==i)
2726 {
2727 el_loads.Erase(l);
2728 elHadLoad = 1;
2729 }
2730 }
2731
2732 ///////////----------- ADJUST ---------------------------//////////////////
2733 // correct the numbers of the loads in the load list of the element
2734 for(int l=1; l<=el_loads.Length(); l++)
2735 {
2736 if(el_loads(l)>=i)
2737 {
2738 el_loads(l)=el_loads(l)-1;
2739 }
2740 }
2741
2742 elP->SetLoadNrs(el_loads);
2743 }
2744
2745 //set new load numbers:
2746 for (int j=i; j <= NLoads(); j++)
2747 {
2748 loads(j)->SetOwnNum(j);
2749 }
2750
2751 // adjust for sensors
2752 for (int j = 1; j <= NSensors(); j++)
2753 {
2754 for (int k = 1; k<=GetSensor(j).GetNumberOfRelatedLoads(); k++)
2755 {
2756 if(GetSensor(j).GetRelatedLoadNumber(k) > i)
2757 {
2758 GetSensor(j).GetRelatedLoadNumber(k)--;
2759 }
2760 }
2761 }
2762 }
2763
DeleteDrawElement(int i)2764 void MultiBodySystem::DeleteDrawElement(int i)
2765 {
2766 ///////////----------- CHECK ---------------------------//////////////////
2767 // check if geom-element is used by an element --> not necessary, just set to 0
2768
2769 ///////////----------- DELETE ---------------------------//////////////////
2770 delete drawelements(i); // just deletes the Element, does not change the length of the TArray
2771 drawelements.Erase(i);
2772 UO(UO_LVL_warn) << "GeomElement "<< i << "deleted\n";
2773
2774 ///////////----------- ADJUST ---------------------------//////////////////
2775 // only elements use GeomElements
2776 for (int j = 1; j <= NE(); j++)
2777 {
2778 for (int k = GetElement(j).NGeomElements(); k >= 1; k--)
2779 {
2780 if (GetElement(j).GetGeomElementNum(k) == i)
2781 {
2782 GetElement(j).GetGeomElementNum(k) = 0;
2783 GetElement(j).SetAltShape(0);
2784 }
2785 else if (GetElement(j).GetGeomElementNum(k) > i)
2786 {
2787 GetElement(j).GetGeomElementNum(k)--;
2788 }
2789 }
2790 }
2791 // adjust of own number not necessary, GeomElements do not know their number!
2792 }
2793
DeleteMaterial(int i)2794 int MultiBodySystem::DeleteMaterial(int i)
2795 {
2796 ///////////----------- CHECK ---------------------------//////////////////
2797 // check if material is used by an element
2798 for (int j = 1; j <= NE(); j++)
2799 {
2800 if(GetElement(j).GetMaterialNum() == i)
2801 {
2802 UO(UO_LVL_err) << "ERROR: Can not delete material " << i << ", because it is still used by element " << j << "!\n";
2803 return 0;
2804 }
2805 }
2806
2807 ///////////----------- DELETE ---------------------------//////////////////
2808 delete materials(i); // just deletes the material, does not change the length of the TArray
2809 materials.Erase(i);
2810 UO(UO_LVL_warn) << "Material "<< i << "deleted\n";
2811
2812 ///////////----------- ADJUST ---------------------------//////////////////
2813 for (int j=i; j <= NMaterials(); j++)
2814 {
2815 materials(j)->SetMaterialNumber(j);
2816 }
2817
2818 // adjust for elements
2819 for (int j = 1; j <= NE(); j++)
2820 {
2821 if (GetElement(j).GetMaterialNum() > i)
2822 {
2823 GetElement(j).GetMaterialNum()--;
2824 }
2825 }
2826 return 1;
2827 }
2828
DeleteNode(int i)2829 void MultiBodySystem::DeleteNode(int i)
2830 {
2831 ///////////----------- CHECK ---------------------------//////////////////
2832 // check if node is used by an element
2833 for (int j = 1; j <= NE(); j++)
2834 {
2835 for (int k = 1; k<=GetElement(j).NNodes(); k++)
2836 {
2837 if(GetElement(j).NodeNum(k) == i)
2838 {
2839 UO(UO_LVL_err) << "ERROR: Can not delete node " << i << ", because it is still used by element " << j << "!\n";
2840 return;
2841 }
2842 }
2843 }
2844
2845 // check if node is used by a sensor
2846 for (int j = 1; j <= NSensors(); j++)
2847 {
2848 for (int k = 1; k<=GetSensor(j).GetNumberOfRelatedNodes(); k++)
2849 {
2850 if(GetSensor(j).GetRelatedNodeNumber(k) == i)
2851 {
2852 UO(UO_LVL_err) << "ERROR: Can not delete node " << i << ", because it is still used by sensor " << j << "!\n";
2853 return;
2854 }
2855 }
2856 }
2857
2858 ///////////----------- DELETE ---------------------------//////////////////
2859 delete nodes(i); // just deletes the Element, does not change the length of the TArray
2860 nodes.Erase(i);
2861 UO(UO_LVL_warn) << "Node "<< i << "deleted\n";
2862
2863 ///////////----------- ADJUST ---------------------------//////////////////
2864 for (int j=i; j <= NNodes(); j++)
2865 {
2866 nodes(j)->NodeNum() = j;
2867 }
2868
2869 // adjust for elements
2870 for (int j = 1; j <= NE(); j++)
2871 {
2872 for (int k = 1; k <= GetElement(j).NNodes(); k++)
2873 {
2874 if (GetElement(j).NodeNum(k) > i)
2875 {
2876 GetElement(j).NodeNum(k)--;
2877 }
2878 }
2879 }
2880
2881 // adjust for sensors
2882 for (int j = 1; j <= NSensors(); j++)
2883 {
2884 for (int k = 1; k<=GetSensor(j).GetNumberOfRelatedNodes(); k++)
2885 {
2886 if(GetSensor(j).GetRelatedNodeNumber(k) > i)
2887 {
2888 GetSensor(j).GetRelatedNodeNumber(k)--;
2889 }
2890 }
2891 }
2892
2893 }
2894 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2895 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2896 // get the i-th sensors (as registered in mbs) own stored values from internal array
GetSensorValuesArrayPtr(int i)2897 TArray<double>* MultiBodySystem::GetSensorValuesArrayPtr(int i)
2898 {
2899 Sensor& the_sensor = GetSensor(i);
2900 TArray<double>* pValues = the_sensor.GetSignalHistoryValuesPtr();
2901 return pValues;
2902 }
2903
2904 // get the i-th sensors (as registered in mbs) own stored times from internal array
GetSensorTimesArrayPtr(int i)2905 TArray<double>* MultiBodySystem::GetSensorTimesArrayPtr(int i)
2906 {
2907 Sensor& the_sensor = GetSensor(i);
2908 TArray<double>* pTimes = the_sensor.GetSignalHistoryTimesPtr();
2909 return pTimes;
2910 }
2911
2912 // get the i-th sensors (as registered in mbs) name
GetSensorName(int i)2913 mystr MultiBodySystem::GetSensorName(int i)
2914 {
2915 return sensors(i)->GetSensorName();
2916 }
2917
2918 // get the number of the (first) sensor that matches the sensorname
GetSensorNumber(mystr & name)2919 int MultiBodySystem::GetSensorNumber(mystr& name)
2920 {
2921 for(int i=1; i<=NSensors(); i++)
2922 {
2923 if(name.Compare(sensors(i)->GetSensorName()))
2924 return i;
2925 }
2926 return 0;
2927 }
2928
2929 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2930 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2931
2932
2933
2934
SetGlobalInitConditions(Vector & x0)2935 void MultiBodySystem::SetGlobalInitConditions(Vector& x0)
2936 {
2937 for (int i=1; i<=elements.Length(); i++)
2938 {
2939 elements(i)->SetGlobalInitConditions(x0);
2940 }
2941 }
2942
SetGlobalInitData(Vector & xdata)2943 void MultiBodySystem::SetGlobalInitData(Vector& xdata)
2944 {
2945 for (int i=1; i<=elements.Length(); i++)
2946 {
2947 elements(i)->SetGlobalInitData(xdata);
2948 }
2949 //$ MaSch 2013-10-28: added support of SPH particle data via auxiliary elements
2950 for (int i=1; i<=auxelements.Length(); i++)
2951 {
2952 if(auxelements(i)->IsType(TParticle))
2953 {
2954 auxelements(i)->SetGlobalInitData(xdata);
2955 }
2956 }
2957 }
2958
NConstraints() const2959 int MultiBodySystem::NConstraints() const
2960 {
2961 int nc = 0;
2962 for (int i=1; i <= NE(); i++) if (elements(i)->IsType(TConstraint)) nc++;
2963 return nc;
2964 }
2965
GetBoundingBoxD() const2966 Box3D MultiBodySystem::GetBoundingBoxD() const
2967 {
2968 Box3D b;
2969 for (int i=1; i<=elements.Length(); i++)
2970 {
2971 b.Add(elements(i)->GetBoundingBoxD());
2972 }
2973
2974 //global_uo << "box1=" << b.PMin() << "," << b.PMax() << "\n";
2975 //bounding box of drawelements that belong to an element is managed in element function!!!
2976 for (int i=1; i<=drawelements.Length(); i++)
2977 {
2978 if (!drawelements(i)->GetElnum())
2979 b.Add(drawelements(i)->GetBoundingBoxD());
2980 }
2981 //global_uo << "box2=" << b.PMin() << "," << b.PMax() << "\n";
2982
2983 // AD 2013-01-18: adapted for new "grid" == scene background
2984 if (GetIOption(126)) //draw grid:
2985 {
2986 double refpos_x = GetDOption(107);
2987 double refpos_y = GetDOption(108);
2988 double refpos_z = GetDOption(109);
2989 double size_of_plane_x = GetDOption(112);
2990 double size_of_plane_y = GetDOption(113);
2991 double size_of_plane_z = GetDOption(142);
2992
2993 Vector3D p1(refpos_x, refpos_y, refpos_z);
2994 Vector3D sz(size_of_plane_x, size_of_plane_y, size_of_plane_z);
2995 Box3D bg(p1, p1+sz);
2996 b.Add(bg);
2997 }
2998
2999 b.Increase(0.001); //original: for better graphics; $JG2013-06-24
3000 b.InflateFactor(1.5); //increase bounding box, such that it fits better into the sceen $JG2013-06-24
3001
3002 return b;
3003 }
3004
3005
3006
CollectAvailableFieldVariables()3007 void MultiBodySystem::CollectAvailableFieldVariables()
3008 {
3009 availableFieldVariables.Flush();
3010
3011 for(int i = 1; i <= elements.Length() + auxelements.Length(); i++)
3012 {
3013 Element * element;
3014 if(i <= elements.Length())
3015 element = elements(i);
3016 else
3017 element = auxelements(i - elements.Length());
3018 TArray<FieldVariableDescriptor> elementVariables;
3019 element->GetAvailableFieldVariables(elementVariables);
3020 FieldVariableDescriptor::MergeArrays(availableFieldVariables, elementVariables);
3021 }
3022
3023 // and now we update the index of the actually selected variable for post-processing
3024 // based on its textual identifier stored in the cfg file
3025 bool found = false;
3026 for(int i = 1; i <= availableFieldVariables.Length(); i++)
3027 if(strcmp(GetTOption(107), availableFieldVariables(i).GetTextualIdentifier()) == 0)
3028 {
3029 SetIndexOfActualPostProcessingFieldVariable(i);
3030 found = true;
3031 break;
3032 }
3033 if(!found)
3034 SetIndexOfActualPostProcessingFieldVariable(0);
3035 }
3036
3037
3038 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
3039 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
3040 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
3041 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
3042 //Procedure for registration of model generation functions in the MBS-System
3043 //$ YV 2013-01-02: the old mechanism is now replaced by the new one: the models are now created in the client dll;
3044 // instead of the functions, which are commented out below, we simply access the models library from the dll
3045 #if 0
3046
3047 const int modelFunctionAutoRegistration_max_n_fct = 256; //number of functions
3048 int modelFunctionAutoRegistration_n_fct = 0; //number of functions
3049 char* modelFunctionAutoRegistration_fname[modelFunctionAutoRegistration_max_n_fct];
3050 char* modelFunctionAutoRegistration_description[modelFunctionAutoRegistration_max_n_fct];
3051 int modelFunctionAutoRegistration_option[modelFunctionAutoRegistration_max_n_fct];
3052 typedef int (*ModelFunctionAutoRegistration_pt2FunctionList)(MBS* mbs);//[modelFunctionAutoRegistration_max_n_fct];
3053 ModelFunctionAutoRegistration_pt2FunctionList modelFunctionAutoRegistration_pt2FunctionList[modelFunctionAutoRegistration_max_n_fct];
3054 ModelFunctionAutoRegistration_pt2FunctionList modelFunctionAutoRegistration_pt2Fcn_InitModelData[modelFunctionAutoRegistration_max_n_fct];
3055
3056 int MultiBodySystem::GetNumberOfModelFunctions() const //return number of available model functions
3057 {
3058 return modelFunctionAutoRegistration_n_fct;
3059 }
3060
3061 char* MultiBodySystem::GetModelFunctionName(int index0) const //return model name string
3062 {
3063 return modelFunctionAutoRegistration_fname[index0];
3064 }
3065
3066 char* MultiBodySystem::GetModelFunctionDescription(int index0) const //return model description string
3067 {
3068 return modelFunctionAutoRegistration_description[index0];
3069 }
3070
3071 int MultiBodySystem::GetModelFunctionOption(int index0) const //return option of model function
3072 {
3073 return modelFunctionAutoRegistration_option[index0];
3074 }
3075
3076 int MultiBodySystem::CallModelFunction(int index0) //Call model function with index "index0"
3077 {
3078 return (modelFunctionAutoRegistration_pt2FunctionList[index0])(this);
3079 }
3080
3081 int MultiBodySystem::CallModelFunction_InitModelData(int index0) //Call model function with index "index0"
3082 {
3083 return (modelFunctionAutoRegistration_pt2Fcn_InitModelData[index0])(this);
3084 }
3085
3086 int MultiBodySystem::ModelFunctionHasInitModelData(int index0) //check if model function has initialization procedure
3087 {
3088 if (modelFunctionAutoRegistration_pt2Fcn_InitModelData[index0] != 0)
3089 {
3090 return 1;
3091 }
3092 else
3093 {
3094 return 0;
3095 }
3096 }
3097
3098 void ModelFunctionAutoRegistration(int(*function_ptr)(MBS* mbs), const char* functionName, const char* description, int option,
3099 int(*function_ptr_init_modeldata)(MBS* mbs))
3100 {
3101 if (modelFunctionAutoRegistration_n_fct < modelFunctionAutoRegistration_max_n_fct)
3102 {
3103 int index = modelFunctionAutoRegistration_n_fct;
3104
3105 //modelFunctionAutoRegistration_pt2Function/*[index]*//* = function_ptr;
3106 modelFunctionAutoRegistration_pt2FunctionList[index] = function_ptr;
3107
3108 //function for initilization of model data:
3109 modelFunctionAutoRegistration_pt2Fcn_InitModelData[index] = function_ptr_init_modeldata;
3110
3111 int length = strlen(functionName);
3112 modelFunctionAutoRegistration_fname[index] = new char[length + 1];
3113 strcpy(modelFunctionAutoRegistration_fname[index], functionName);
3114
3115 length = strlen(description);
3116 modelFunctionAutoRegistration_description[index] = new char[length + 1];
3117 strcpy(modelFunctionAutoRegistration_description[index], description);
3118
3119 modelFunctionAutoRegistration_option[index] = option;
3120
3121 //increase index after all initialization!
3122 modelFunctionAutoRegistration_n_fct++;
3123 }
3124 }
3125
3126 #endif
3127
ModelFunctionChanged()3128 void MultiBodySystem::ModelFunctionChanged()
3129 {
3130 SetModelData_Initialized(0);
3131 Initialize();
3132 }
3133
3134
GetModelFunctionsIndex0()3135 int MultiBodySystem::GetModelFunctionsIndex0()
3136 {
3137 int ind0 = -1;
3138 int nmodel = GetModelsLibrary()->GetModelsCount();
3139
3140 HOTINTOptions hotint_options_nonconstant_copy(*GetOptions());
3141 mystr model_str = hotint_options_nonconstant_copy.GeneralOptions()->ModelFileInternalModelFunctionName();
3142
3143 //$ YV 2013-01-04: model indices are 1-based
3144 for (int i=1; i <= nmodel; i++)
3145 {
3146 //UO() << "Model " << i << "= '" << GetModelFunctionName(i) << "', description='" << GetModelFunctionDescription(i) << "'\n";
3147 if (model_str == GetModelsLibrary()->GetModelInterface(i)->GetMBSModelName())
3148 {
3149 ind0 = i;
3150 }
3151 }
3152 return ind0;
3153 }
3154 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
3155 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
3156
3157
3158
3159
3160
EvaluateParsedFunction1D(const mystr & parsedFunctionExpression,const mystr & parsedFunctionVariables,double t)3161 double MultiBodySystem::EvaluateParsedFunction1D(const mystr & parsedFunctionExpression, const mystr & parsedFunctionVariables, double t)
3162 {
3163 CParsedFunction pf;
3164 //$ YV 2012-13-12: changed MBSParser() to &MBSParser()
3165 pf.SetParsedFunction1D(&MBSParser(), parsedFunctionExpression, parsedFunctionVariables);
3166 return pf.Evaluate(t);
3167 }
3168
ExpressionToDouble(mystr & expression)3169 double MultiBodySystem::ExpressionToDouble(mystr & expression)
3170 {
3171 int error;
3172 return MBSParser().ExpressionToDouble(expression, error);
3173 }
3174
3175 //$ DR 2013-01-14: added as new interface to script parser
ExecuteParserCommand(mystr & command,TArray<ElementDataContainer * > & parameter_EDCs,ElementData & return_value,int option)3176 int MultiBodySystem::ExecuteParserCommand(mystr & command, TArray<ElementDataContainer*>& parameter_EDCs, ElementData& return_value, int option)
3177 {
3178 return EDCParser().ExecuteParserCommand(command, parameter_EDCs, return_value, option);
3179 }
3180