MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
modal.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/modal.cc,v 1.184 2017/01/12 14:46:43 masarati Exp $ */
2 /*
3  * MBDyn (C) is a multibody analysis code.
4  * http://www.mbdyn.org
5  *
6  * Copyright (C) 1996-2017
7  *
8  * Pierangelo Masarati <masarati@aero.polimi.it>
9  * Paolo Mantegazza <mantegazza@aero.polimi.it>
10  *
11  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
12  * via La Masa, 34 - 20156 Milano, Italy
13  * http://www.aero.polimi.it
14  *
15  * Changing this copyright notice is forbidden.
16  *
17  * This program is free software; you can redistribute it and/or modify
18  * it under the terms of the GNU General Public License as published by
19  * the Free Software Foundation (version 2 of the License).
20  *
21  *
22  * This program is distributed in the hope that it will be useful,
23  * but WITHOUT ANY WARRANTY; without even the implied warranty of
24  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
25  * GNU General Public License for more details.
26  *
27  * You should have received a copy of the GNU General Public License
28  * along with this program; if not, write to the Free Software
29  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30  */
31 
32 /*
33  classe per l'introduzione della flessibilita' modale nel codice multi-corpo
34 
35  29/7/99: implementata la parte di corpo rigido (solo massa e momenti
36  d'inerzia) verificata con massa isolata soggetta a forze
37  e momenti esterni
38 
39  13/9/99: corpo rigido OK (anche momenti statici ed eq. di vincolo)
40  corpo deformabile isolato OK (verificato con osc. armonico)
41 
42  23/9/99: corpo deformabile + eq. di vincolo (no moto rigido) OK
43  (verificato con trave incastrata)
44  corpo rigido + deformabile OK (verificato con trave libera)
45 
46  10/99: validazioni con NASTRAN: trave incastrata, trave libera
47  con forza in mezzeria, trave libera con forza all'estremita'
48 
49  4/11/99: completate le funzioni InitialAssJac e InitialAssRes
50  per l'assemblaggio iniziale del 'vincolo'
51 
52  22/11/99: modificata la funzione ReadModal per leggere il file
53  generato da DADS
54 
55  26/11/99: validazione con piastra vincolata con elementi elastici
56 
57  12/99: validazione con piastra e bauchau
58 
59  01/2000: modi rotanti
60 
61  30/11/99: aggiunto lo smorzamento strutturale tra i dati d'ingresso
62  aggiunte le inerzie concentrate
63 
64  1/12/99: modifiche alla lettura dati d'ingresso (l'estensione *.fem
65  al file con i dati del modello ad elementi viene aggiunta
66  automaticamente dal programma, che crea un file di
67  output con l'estensione *.mod)
68  corretto un bug nella scrittura delle equazioni di vincolo
69 
70  17/12/99: aggiunta la possibilita' di definire uno smorzamento strutturale
71  variabile con le frequenze
72 
73  23/12/99: nuova modifica alla lettura dati di ingresso
74 
75 10/01/2000: introdotta la possibilita' di definire un fattore di scala
76  per i dati del file d'ingresso
77 
78 22/01/2000: tolto il fattore di scala perche' non funziona
79 
80  03/2000: aggiunte funzioni che restituiscono dati dell'elemento
81  (autovettori, modi ecc.) aggiunta la possibilita'
82  di imporre delle deformate modali iniziali
83 
84  Modifiche fatte al resto del programma:
85 
86  Nella classe joint : aggiunto il vincolo modale
87  Nella classe strnode : aggiunto il nodo modale
88  Nella classe MatVec3n : aggiunte classi e funzioni per gestire matrici
89  Nx3 e NxN
90  Nella classe SubMat : corretto un bug nelle funzioni Add Mat3xN ecc.,
91  aggiunte funzioni per trattare sottomatrici Nx3
92 */
93 
94 /*
95  * Copyright 1999-2017 Felice Felippone <ffelipp@tin.it>
96  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
97  */
98 
99 /*
100  * Copyright 1999-2017 Pierangelo Masarati <masarati@aero.polimi.it>
101  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
102  *
103  * Modified by Pierangelo Masarati
104  */
105 
106 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
107 
108 /* FIXME: gravity in modal elements is eXperimental; undefine to disable */
109 #define MODAL_USE_GRAVITY
110 
111 #include <cerrno>
112 #include <cstring>
113 #include <stdint.h>
114 #include <sys/stat.h>
115 #include <limits>
116 #include <algorithm>
117 
118 #include "modal.h"
119 #include "dataman.h"
120 #include "Rot.hh"
121 #include "hint_impl.h"
122 
123 Modal::Modal(unsigned int uL,
124  const ModalNode* pR,
125  const Vec3& x0,
126  const Mat3x3& R0,
127  const DofOwner* pDO,
128  unsigned int NM, /* numero modi */
129  unsigned int NI, /* numero nodi d'interfaccia */
130  unsigned int NF, /* numero nodi FEM */
131  doublereal dMassTmp, /* inv. inerzia (m, m.stat., d'in.) */
132  const Vec3& STmp,
133  const Mat3x3& JTmp,
134  const std::vector<unsigned int>& uModeNumber,
135  MatNxN *pGenMass,
136  MatNxN *pGenStiff,
137  MatNxN *pGenDamp,
138  const std::vector<std::string>& IdFEMNodes, /* label nodi FEM */
139  Mat3xN *pN, /* posizione dei nodi FEM */
140  const std::vector<Modal::StrNodeData>& snd,
141  Mat3xN *pPHItStrNode, /* forme modali nodi d'interfaccia */
142  Mat3xN *pPHIrStrNode,
143  Mat3xN *pModeShapest, /* autovettori: servono a aeromodal */
144  Mat3xN *pModeShapesr,
145  Mat3xN *pInv3, /* invarianti d'inerzia I3...I11 */
146  Mat3xN *pInv4,
147  Mat3xN *pInv5,
148  Mat3xN *pInv8,
149  Mat3xN *pInv9,
150  Mat3xN *pInv10,
151  Mat3xN *pInv11,
152  VecN *aa,
153  VecN *bb,
154  flag fOut)
155 : Elem(uL, fOut),
156 Joint(uL, pDO, fOut),
157 pModalNode(pR),
158 iRigidOffset(pModalNode ? 12 : 0),
159 x(x0),
160 R(R0),
161 RT(R0.Transpose()),
162 NModes(NM),
163 NStrNodes(NI),
164 NFEMNodes(NF),
165 IdFEMNodes(IdFEMNodes),
166 pXYZFEMNodes(pN),
167 dMass(dMassTmp),
168 Inv2(STmp),
169 Inv7(JTmp),
170 uModeNumber(uModeNumber),
171 pModalMass(pGenMass),
172 pModalStiff(pGenStiff),
173 pModalDamp(pGenDamp),
174 pPHIt(pPHItStrNode),
175 pPHIr(pPHIrStrNode),
176 pModeShapest(pModeShapest),
177 pModeShapesr(pModeShapesr),
178 pCurrXYZ(0),
179 pCurrXYZVel(0),
180 pInv3(pInv3),
181 pInv4(pInv4),
182 pInv5(pInv5),
183 pInv8(pInv8),
184 pInv9(pInv9),
185 pInv10(pInv10),
186 pInv11(pInv11),
187 Inv3jaj(::Zero3),
188 #if !(USE_AUTODIFF && MODAL_USE_AUTODIFF) || MODAL_DEBUG_AUTODIFF
189 Inv3jaPj(::Zero3),
190 #endif
191 Inv8jaj(::Zero3x3),
192 #if !(USE_AUTODIFF && MODAL_USE_AUTODIFF) || MODAL_DEBUG_AUTODIFF
193 Inv8jaPj(::Zero3x3),
194 Inv5jaj(NModes, 0.),
195 Inv5jaPj(NModes, 0.),
196 #endif
197 Inv9jkajak(::Eye3),
198 #if !(USE_AUTODIFF && MODAL_USE_AUTODIFF) || MODAL_DEBUG_AUTODIFF
199 Inv9jkajaPk(::Eye3),
200 #endif
201 a(*aa), a0(*aa),
202 aPrime(NModes, 0.), aPrime0(*bb),
203 b(*bb),
204 bPrime(NModes, 0.),
205 SND(snd)
206 {
207  ASSERT(pModalNode->GetStructNodeType() == StructNode::MODAL);
208 }
209 
210 Modal::~Modal(void)
211 {
212  if (pXYZFEMNodes) {
213  SAFEDELETE(pXYZFEMNodes);
214  }
215  if (pModalMass) {
216  SAFEDELETE(pModalMass);
217  }
218  if (pModalStiff) {
219  SAFEDELETE(pModalStiff);
220  }
221  if (pModalDamp) {
222  SAFEDELETE(pModalDamp);
223  }
224  if (pModeShapest) {
225  SAFEDELETE(pModeShapest);
226  }
227  if (pModeShapesr) {
228  SAFEDELETE(pModeShapesr);
229  }
230  if (pPHIt) {
231  SAFEDELETE(pPHIt);
232  }
233  if (pPHIr) {
234  SAFEDELETE(pPHIr);
235  }
236  if (pInv3) {
237  SAFEDELETE(pInv3);
238  }
239  if (pInv4) {
240  SAFEDELETE(pInv4);
241  }
242  if (pInv5) {
243  SAFEDELETE(pInv5);
244  }
245  if (pInv8) {
246  SAFEDELETE(pInv8);
247  }
248  if (pInv9) {
249  SAFEDELETE(pInv9);
250  }
251  if (pInv10) {
252  SAFEDELETE(pInv10);
253  }
254  if (pInv11) {
255  SAFEDELETE(pInv11);
256  }
257 
258  /* FIXME: destroy all the other data ... */
259 }
260 
261 Joint::Type
262 Modal::GetJointType(void) const
263 {
264  return Joint::MODAL;
265 }
266 
267 std::ostream&
268 Modal::Restart(std::ostream& out) const
269 {
270  return out << "modal; # not implemented yet" << std::endl;
271 }
272 
273 unsigned int
274 Modal::iGetNumDof(void) const
275 {
276  /* i gradi di liberta' propri sono:
277  * 2*M per i modi
278  * 6*N per le reazioni vincolari dei nodi d'interfaccia */
279  return 2*NModes + 6*NStrNodes;
280 }
281 
282 std::ostream&
283 Modal::DescribeDof(std::ostream& out, const char *prefix, bool bInitial) const
284 {
285  integer iModalIndex = iGetFirstIndex();
286 
287  out
288  << prefix << iModalIndex + 1 << "->" << iModalIndex + NModes
289  << ": modal displacements [q(1.." << NModes << ")]" << std::endl
290  << prefix << iModalIndex + NModes + 1 << "->" << iModalIndex + 2*NModes
291  << ": modal velocities [qP(1.." << NModes << ")]" << std::endl;
292  iModalIndex += 2*NModes;
293  for (unsigned iStrNodem1 = 0; iStrNodem1 < NStrNodes; iStrNodem1++, iModalIndex += 6) {
294  out
295  << prefix << iModalIndex + 1 << "->" << iModalIndex + 3 << ": "
296  "StructNode(" << SND[iStrNodem1].pNode->GetLabel() << ") "
297  "reaction forces [Fx,Fy,Fz]" << std::endl
298  << prefix << iModalIndex + 4 << "->" << iModalIndex + 6 << ": "
299  "StructNode(" << SND[iStrNodem1].pNode->GetLabel() << ") "
300  "reaction couples [Mx,My,Mz]" << std::endl;
301  if (bInitial) {
302  iModalIndex += 6;
303  out
304  << prefix << iModalIndex + 1 << "->" << iModalIndex + 3 << ": "
305  "StructNode(" << SND[iStrNodem1].pNode->GetLabel() << ") "
306  "reaction force derivatives [FPx,FPy,FPz]" << std::endl
307  << prefix << iModalIndex + 4 << "->" << iModalIndex + 6 << ": "
308  "StructNode(" << SND[iStrNodem1].pNode->GetLabel() << ") "
309  "reaction couple derivatives [MPx,MPy,MPz]" << std::endl;
310  }
311  }
312 
313  return out;
314 }
315 
316 static const char xyz[] = "xyz";
317 static const char *mdof[] = {
318  "modal displacement q",
319  "modal velocity qP"
320 };
321 static const char *rdof[] = {
322  "reaction force F",
323  "reaction couple M",
324  "reaction force derivative FP",
325  "reaction couple derivative MP"
326 };
327 static const char *meq[] = {
328  "modal velocity definition q",
329  "modal equilibrium equation f"
330 };
331 static const char *req[] = {
332  "position constraint P",
333  "orientation constraint g",
334  "velocity constraint v",
335  "angular velocity constraint w"
336 };
337 
338 void
339 Modal::DescribeDof(std::vector<std::string>& desc, bool bInitial, int i) const
340 {
341  int iend = 1;
342  if (i == -1) {
343  iend = 2*NModes + 6*NStrNodes;
344  if (bInitial) {
345  iend += 6*NStrNodes;
346  }
347  }
348  desc.resize(iend);
349 
350  unsigned modulo = 6;
351  if (bInitial) {
352  modulo = 12;
353  }
354 
355  std::ostringstream os;
356  os << "Modal(" << GetLabel() << ")";
357 
358  if (i < -1 ) {
359  // error
360  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
361 
362  } else if (i == -1) {
363  std::string name(os.str());
364 
365  for (unsigned i = 0; i < 2*NModes; i++) {
366  os.str(name);
367  os.seekp(0, std::ios_base::end);
368  os << ": " << mdof[i/NModes] << "(" << i%NModes + 1 << ")";
369  desc[i] = os.str();
370  }
371 
372  for (unsigned i = 0; i < modulo*NStrNodes; i++) {
373  os.str(name);
374  os.seekp(0, std::ios_base::end);
375  os << ": StructNode(" << SND[i/modulo].pNode->GetLabel() << ") "
376  << rdof[(i/3)%(modulo/3)] << xyz[i%3];
377  desc[2*NModes + i] = os.str();
378  }
379 
380  } else {
381  if (unsigned(i) < 2*NModes) {
382  // modes
383  os << ": " << mdof[i/NModes] << "(" << i%NModes + 1 << ")";
384 
385  } else {
386  // reactions
387  i -= 2*NModes;
388 
389  if (unsigned(i) >= modulo*NStrNodes) {
390  // error
391  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
392  }
393 
394  os << ": StructNode(" << SND[i/modulo].pNode->GetLabel() << ") "
395  << rdof[(i/3)%(modulo/3)] << xyz[i%3];
396  }
397  desc[0] = os.str();
398  }
399 }
400 
401 std::ostream&
402 Modal::DescribeEq(std::ostream& out, const char *prefix, bool bInitial) const
403 {
404  integer iModalIndex = iGetFirstIndex();
405 
406  out
407  << prefix << iModalIndex + 1 << "->" << iModalIndex + NModes
408  << ": modal velocity definitions [q(1.." << NModes << ")=qP(1.." << NModes << ")]" << std::endl
409  << prefix << iModalIndex + NModes + 1 << "->" << iModalIndex + 2*NModes
410  << ": modal equilibrium equations [1.." << NModes << "]" << std::endl;
411  iModalIndex += 2*NModes;
412  for (unsigned iStrNodem1 = 0; iStrNodem1 < NStrNodes; iStrNodem1++, iModalIndex += 6) {
413  out
414  << prefix << iModalIndex + 1 << "->" << iModalIndex + 3 << ": "
415  "StructNode(" << SND[iStrNodem1].pNode->GetLabel() << ") "
416  "position constraints [Px=PxN,Py=PyN,Pz=PzN]" << std::endl
417  << prefix << iModalIndex + 4 << "->" << iModalIndex + 6 << ": "
418  "StructNode(" << SND[iStrNodem1].pNode->GetLabel() << ") "
419  "orientation constraints [gx=gxN,gy=gyN,gz=gzN]" << std::endl;
420  if (bInitial) {
421  iModalIndex += 6;
422  out
423  << prefix << iModalIndex + 1 << "->" << iModalIndex + 3 << ": "
424  "StructNode(" << SND[iStrNodem1].pNode->GetLabel() << ") "
425  "velocity constraints [vx=vxN,vy=vyN,vz=vzN]" << std::endl
426  << prefix << iModalIndex + 4 << "->" << iModalIndex + 6 << ": "
427  "StructNode(" << SND[iStrNodem1].pNode->GetLabel() << ") "
428  "angular velocity constraints [wx=wxN,wy=wyN,wz=wzN]" << std::endl;
429  }
430  }
431 
432  return out;
433 }
434 
435 void
436 Modal::DescribeEq(std::vector<std::string>& desc, bool bInitial, int i) const
437 {
438  int iend = 1;
439  if (i == -1) {
440  iend = 2*NModes + 6*NStrNodes;
441  if (bInitial) {
442  iend += 6*NStrNodes;
443  }
444  }
445  desc.resize(iend);
446 
447  unsigned modulo = 6;
448  if (bInitial) {
449  modulo = 12;
450  }
451 
452  std::ostringstream os;
453  os << "Modal(" << GetLabel() << ")";
454 
455  if (i < -1 ) {
456  // error
457  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
458 
459  } else if (i == -1) {
460  std::string name(os.str());
461 
462  for (unsigned i = 0; i < 2*NModes; i++) {
463  os.str(name);
464  os.seekp(0, std::ios_base::end);
465  os << ": " << meq[i/NModes] << "(" << i%NModes + 1 << ")";
466  desc[i] = os.str();
467  }
468 
469  for (unsigned i = 0; i < modulo*NStrNodes; i++) {
470  os.str(name);
471  os.seekp(0, std::ios_base::end);
472  os << ": StructNode(" << SND[i/modulo].pNode->GetLabel() << ") "
473  << req[(i/3)%(modulo/3)] << xyz[i%3];
474  desc[2*NModes + i] = os.str();
475  }
476 
477  } else {
478  if (unsigned(i) < 2*NModes) {
479  // modes
480  os << ": " << meq[i/NModes] << "(" << i%NModes + 1 << ")";
481 
482  } else {
483  // reactions
484  i -= 2*NModes;
485 
486  if (unsigned(i) >= modulo*NStrNodes) {
487  // error
488  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
489  }
490 
491  os << ": StructNode(" << SND[i/modulo].pNode->GetLabel() << ") "
492  << req[(i/3)%(modulo/3)] << xyz[i%3];
493  }
494  desc[0] = os.str();
495  }
496 }
497 
498 DofOrder::Order
499 Modal::GetDofType(unsigned int i) const
500 {
501  ASSERT(i < iGetNumDof());
502 
503  if (i < 2*NModes) {
504  /* gradi di liberta' differenziali (eq. modali) */
505  return DofOrder::DIFFERENTIAL;
506 
507  } /* else if (i >= 2*NModes && i < iGetNumDof()) { */
508  /* gradi di liberta' algebrici (eq. di vincolo) */
509  return DofOrder::ALGEBRAIC;
510  /* } */
511 }
512 
513 DofOrder::Order
514 Modal::GetEqType(unsigned int i) const
515 {
516  ASSERT(i < iGetNumDof());
517 
518  if (i < 2*NModes) {
519  /* gradi di liberta' differenziali (eq. modali) */
520  return DofOrder::DIFFERENTIAL;
521 
522  } /* else if (i >= 2*NModes && i < iGetNumDof()) { */
523  /* gradi di liberta' algebrici (eq. di vincolo) */
524  return DofOrder::ALGEBRAIC;
525  /* } */
526 }
527 
528 void
529 Modal::WorkSpaceDim(integer* piNumRows, integer* piNumCols) const
530 {
531 #if USE_AUTODIFF && MODAL_USE_AUTODIFF && !MODAL_DEBUG_AUTODIFF
532  const integer iNumRows = (pModalNode ? 12 : 0) + 2 * NModes + 2 * NModes * NStrNodes + 12 * NStrNodes;
533  const integer iNumCols = (pModalNode ? 6 : 0) + 2 * NModes + NStrNodes * 12;
534  *piNumRows = -iNumRows;
535  *piNumCols = iNumCols;
536 #else
537  /* la matrice e' gestita come piena (c'e' un po' di spreco...) */
538  *piNumCols = *piNumRows = iRigidOffset + iGetNumDof() + 6*NStrNodes;
539 #endif
540 }
541 
542 VariableSubMatrixHandler&
543 Modal::AssJac(VariableSubMatrixHandler& WorkMat,
544  doublereal dCoef,
545  const VectorHandler& XCurr,
546  const VectorHandler& XPrimeCurr)
547 {
548  DEBUGCOUT("Entering Modal::AssJac()" << std::endl);
549 #if USE_AUTODIFF && MODAL_USE_AUTODIFF && !MODAL_DEBUG_AUTODIFF
550  SparseSubMatrixHandler& WorkMatSp = WorkMat.SetSparse();
551 
552  grad::GradientAssVec<grad::Gradient<0> >::AssJac(this,
553  WorkMatSp,
554  dCoef,
555  XCurr,
556  XPrimeCurr,
557  grad::REGULAR_JAC,
558  &dofMap);
559 #else
560  FullSubMatrixHandler& WM = WorkMat.SetFull();
561  integer iNumRows = 0;
562  integer iNumCols = 0;
563  WorkSpaceDim(&iNumRows, &iNumCols);
564  WM.ResizeReset(iNumRows, iNumCols);
565 
566  /* gli indici sono ordinati cosi': i primi 6 sono le equazioni
567  * per abbassare di grado il sistema,
568  * quindi le 6 equazioni del moto rigido, quindi le 2*M modali,
569  * quindi le eq. vincolari */
570 
571  /* FIXME: by now, I add a test everywhere it's needed;
572  * later, I'll try to group conditional parts in separate tests */
573 
574  /* indici della parte rigida */
575 
576  if (pModalNode) {
577  integer iRigidIndex = pModalNode->iGetFirstIndex();
578 
579  for (unsigned int iCnt = 1; iCnt <= iRigidOffset; iCnt++) {
580  WM.PutRowIndex(iCnt, iRigidIndex + iCnt);
581  WM.PutColIndex(iCnt, iRigidIndex + iCnt);
582  }
583  }
584 
585  /* indici della parte deformabile e delle reazioni vincolari */
586  integer iFlexIndex = iGetFirstIndex();
587  unsigned int iNumDof = iGetNumDof();
588 
589  for (unsigned int iCnt = 1; iCnt <= iNumDof; iCnt++) {
590  WM.PutRowIndex(iRigidOffset + iCnt, iFlexIndex + iCnt);
591  WM.PutColIndex(iRigidOffset + iCnt, iFlexIndex + iCnt);
592  }
593 
594  /* indici delle equazioni vincolari (solo per il nodo 2) */
595  for (unsigned int iStrNodem1 = 0; iStrNodem1 < NStrNodes; iStrNodem1++) {
596  integer iNodeFirstMomIndex =
597  SND[iStrNodem1].pNode->iGetFirstMomentumIndex();
598  integer iNodeFirstPosIndex =
599  SND[iStrNodem1].pNode->iGetFirstPositionIndex();
600 
601  integer iOffset = iRigidOffset + iNumDof + 6*iStrNodem1;
602  for (integer iCnt = 1; iCnt <= 6; iCnt++) {
603  WM.PutRowIndex(iOffset + iCnt, iNodeFirstMomIndex + iCnt);
604  WM.PutColIndex(iOffset + iCnt, iNodeFirstPosIndex + iCnt);
605  }
606  }
607 
608  /* Assemblaggio dello Jacobiano */
609 
610  Vec3 wr(::Zero3);
611  Mat3x3 J(::Zero3x3);
612  Vec3 S(::Zero3);
613  if (pModalNode) {
614  /* recupera e aggiorna i dati necessari */
615  /* i prodotti Inv3j*aj ecc. sono gia' stati fatti da AssRes() */
616 
617  wr = pModalNode->GetWRef();
618  /* R and RT are updated by AssRes() */
619  Mat3x3 JTmp = Inv7;
620  if (pInv8 != 0) {
621  JTmp += Inv8jaj.Symm2();
622  }
623  J = R*JTmp*RT;
624  Vec3 STmp = Inv2;
625  if (pInv3 != 0) {
626  STmp += Inv3jaj;
627  }
628  S = R*STmp;
629 
630  /* matrice di massa: J[1,1] = Mtot */
631  for (int iCnt = 6 + 1; iCnt <= 6 + 3; iCnt++) {
632  WM.PutCoef(iCnt, iCnt, dMass);
633  }
634 
635  /* momenti statici J[1,2] = -[S/\] + c[-2w/\S/\ + S/\w/\] */
636  WM.Add(6 + 1, 9 + 1, Mat3x3(MatCrossCross, S, wr*dCoef)
637  - Mat3x3(MatCrossCross, wr, S*(2.*dCoef))
638  - Mat3x3(MatCross, S));
639 
640  /* J[2,1] = [S/\] */
641  WM.Add(9 + 1, 6 + 1, Mat3x3(MatCross, S));
642 
643  /* momenti d'inerzia: J[2,2] = J + c[ - (Jw)/\ + (w/\)J]; */
644  WM.Add(9 + 1, 9 + 1, J + ((wr.Cross(J) - Mat3x3(MatCross, J*wr))*dCoef));
645 
646  /* completa lo Jacobiano con l'aggiunta delle equazioni {xP} = {v}
647  {gP} - [wr/\]{g} = {w} */
648  for (int iCnt = 1; iCnt <= 6; iCnt++) {
649  WM.IncCoef(iCnt, iCnt, 1.);
650  WM.DecCoef(iCnt, 6 + iCnt, dCoef);
651  }
652  WM.Sub(3 + 1, 3 + 1, Mat3x3(MatCross, wr*dCoef));
653  }
654 
655  /* parte deformabile :
656  *
657  * | I -cI ||aP|
658  * | || |
659  * |cK M + cC ||bP|
660  */
661 
662  for (unsigned int iCnt = 1; iCnt <= NModes; iCnt++) {
663  unsigned int iiCnt = iRigidOffset + iCnt;
664 
665  WM.PutCoef(iiCnt, iiCnt, 1.);
666  WM.PutCoef(iiCnt, iiCnt + NModes, -dCoef);
667  for (unsigned int jCnt = 1; jCnt <= NModes; jCnt++) {
668  unsigned int jjCnt = iRigidOffset + jCnt;
669 
670  WM.PutCoef(iiCnt + NModes, jjCnt,
671  dCoef*pModalStiff->dGet(iCnt, jCnt));
672  WM.PutCoef(iiCnt + NModes, jjCnt + NModes,
673  pModalMass->dGet(iCnt, jCnt)
674  + dCoef*pModalDamp->dGet(iCnt, jCnt));
675  }
676  }
677 
678  if (pModalNode) {
679 
680  /* termini di accoppiamento moto rigido-deformabile;
681  * eventualmente l'utente potra' scegliere
682  * se trascurarli tutti, una parte o considerarli tutti */
683 
684  /* linearizzazione delle OmegaPrime:
685  * J13 = R*Inv3jaj
686  * J23 = R*Inv4 + Inv5jaj
687  * (questi termini ci vogliono sempre)
688  */
689 
690  if (pInv3 != 0) {
691  Mat3xN Jac13(NModes, 0.);
692  WM.Add(6 + 1, iRigidOffset + NModes + 1, Jac13.LeftMult(R, *pInv3));
693 
694  WM.AddT(iRigidOffset + NModes + 1, 6 + 1, Jac13);
695 
696  Mat3x3 Inv3jaPjWedge(MatCross, R*(Inv3jaPj*(2.*dCoef)));
697  WM.Sub(6 + 1, 9 + 1, Inv3jaPjWedge);
698  }
699 
700  if (pInv4 != 0) {
701  Mat3xN Jac23(NModes, 0.);
702  Jac23.LeftMult(R, *pInv4);
703  if (pInv5 != 0) {
704  Mat3xN Inv5jajRef(NModes, 0.);
705 
706  Jac23 += Inv5jajRef.LeftMult(R, Inv5jaj);
707  }
708  WM.Add(9 + 1, iRigidOffset + NModes + 1, Jac23);
709 
710  WM.AddT(iRigidOffset + NModes + 1, 9 + 1, Jac23);
711  }
712 
713 
714  /* termini di Coriolis: linearizzazione delle Omega
715  * (si puo' evitare se non ho grosse vel. angolari):
716  * J13 = -2*R*[Inv3jaPj/\]*RT
717  * J23 = 2*R*[Inv8jaPj - Inv9jkajaPk]*RT */
718 
719  if (pInv8 != 0 ) {
720  Mat3x3 JTmp = Inv8jaPj;
721  if (pInv9 != 0) {
722  JTmp -= Inv9jkajaPk;
723  }
724  WM.Add(9 + 1, 9 + 1, R*(JTmp*(RT*(2.*dCoef))));
725  }
726 
727  /* termini di Coriolis: linearizzazione delle b;
728  * si puo' evitare 'quasi' sempre: le velocita'
729  * di deformazione dovrebbero essere sempre piccole
730  * rispetto ai termini rigidi
731  * Jac13 = 2*[Omega/\]*R*PHI
732  */
733 #if 0
734  Jac13.LeftMult(wrWedge*R*2*dCoef, *pInv3);
735  WM.Add(6 + 1, iRigidOffset + NModes + 1, Jac13);
736 #endif
737  /* nota: non riesco a tirar fuori la Omega dall'equazione
738  * dei momenti:
739  * 2*[ri/\]*[Omega/\]*R*PHIi*{DeltaaP}, i = 1,...nnodi
740  * quindi questa equazione non si puo' linearizzare
741  * a meno di ripetere la sommatoria estesa a tutti i nodi
742  * a ogni passo del Newton-Rapson... */
743 
744  /* linearizzazione delle forze centrifughe e di Coriolis
745  * in base modale (anche questi termini dovrebbero essere
746  * trascurabili) */
747 #if 0
748  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
749  Inv4j = pInv4->GetVec(jMode);
750  VInv5jaj = Inv5jaj.GetVec(jMode);
751  VInv5jaPj = Inv5jaPj.GetVec(jMode);
752  unsigned int jOffset = (jMode - 1)*3 + 1;
753  Inv8jTranspose = (pInv8->GetMat3x3(jOffset)).Transpose();
754  if (pInv9 != 0 ) {
755  Inv9jkak = 0.;
756  for (unsigned int kModem1 = 0; kModem1 < NModes; kModem1++) {
757  doublereal a_kMode = a.dGet(kModem1 + 1);
758  integer iOffset = (jMode - 1)*3*NModes + kModem1*3 + 1;
759  Inv9jkak += pInv9->GetMat3x3ScalarMult(iOffset, a_kMode);
760  }
761  }
762  for (int iCnt = 1; iCnt <= 3; iCnt++) {
763  doublereal temp1 = 0., temp2 = 0.;
764  for (int jCnt = 1; jCnt<=3; jCnt++) {
765  if (pInv9 != 0) {
766  temp1 += -2*wr.dGet(jCnt)*(R*(Inv8jTranspose - Inv9jkak)*RT).dGet(iCnt, jCnt);
767  } else {
768  temp1 += -2*wr.dGet(jCnt)*(R*(Inv8jTranspose)*RT).dGet(iCnt, jCnt);
769  }
770  temp2 += -(R*(Inv4j + VInv5jaj)).dGet(jCnt)*wrWedge.dGet(iCnt, jCnt);
771  }
772  WM.IncCoef(iRigidOffset + NModes + jMode, 9 + iCnt,
773  dCoef*(temp1 + temp2));
774  }
775  for (int iCnt = 1; iCnt<=3; iCnt++) {
776  doublereal temp1 = 0.;
777  for (int jCnt = 1; jCnt<=3; jCnt++) {
778  temp1 += (R*VInv5jaPj*2).dGet(jCnt);
779  }
780  WM.IncCoef(iRigidOffset + NModes + jMode, 9 + iCnt,
781  dCoef*temp1);
782  }
783  }
784 #endif
785  }
786 
787  /* ciclo esteso a tutti i nodi d'interfaccia */
788  for (unsigned int iStrNode = 1; iStrNode <= NStrNodes; iStrNode++) {
789  unsigned int iStrNodem1 = iStrNode - 1;
790 
791  /* recupero le forme modali del nodo vincolato */
792  Mat3xN PHIt(NModes), PHIr(NModes);
793  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
794  integer iOffset = (jMode - 1)*NStrNodes + iStrNode;
795 
796  PHIt.PutVec(jMode, pPHIt->GetVec(iOffset));
797  PHIr.PutVec(jMode, pPHIr->GetVec(iOffset));
798  }
799 
800  MatNx3 PHItT(NModes), PHIrT(NModes);
801  PHItT.Transpose(PHIt);
802  PHIrT.Transpose(PHIr);
803 
804  /* nota: le operazioni
805  * d1tot = d1 + PHIt*a, R1tot = R*[I + (PHIr*a)/\]
806  * sono gia' state fatte da AssRes */
807 
808  Mat3xN SubMat1(NModes), SubMat2(NModes);
809  MatNx3 SubMat3(NModes);
810  MatNxN SubMat4(NModes);
811 
812  /* cerniera sferica */
813  /* F e' aggiornata da AssRes */
814 
815  integer iReactionIndex = iRigidOffset + 2*NModes + 6*iStrNodem1;
816  integer iStrNodeIndex = iRigidOffset + iNumDof + 6*iStrNodem1;
817 
818  Mat3x3 FTmpWedge(MatCross, SND[iStrNodem1].F*dCoef);
819 
820  Mat3xN PHItR(NModes);
821  PHItR.LeftMult(R, PHIt);
822 
823  for (int iCnt = 1; iCnt <= 3; iCnt++) {
824  /* termini di reazione sui nodi */
825  WM.DecCoef(iStrNodeIndex + iCnt, iReactionIndex + iCnt, 1.);
826 
827  /* termini di vincolo dovuti ai nodi */
828  WM.DecCoef(iReactionIndex + iCnt, iStrNodeIndex + iCnt, 1.);
829  }
830 
831  if (pModalNode) {
832  for (int iCnt = 1; iCnt <= 3; iCnt++) {
833  /* termini di reazione sul nodo modale */
834  WM.IncCoef(6 + iCnt, iReactionIndex + iCnt, 1.);
835 
836  /* termini di vincolo dovuti al nodo modale */
837  WM.IncCoef(iReactionIndex + iCnt, iCnt, 1.);
838  }
839 
840  /* pd1Tot e' il puntatore all'array
841  * che contiene le posizioni del nodi FEM */
842  Mat3x3 dTmp1Wedge(MatCross, SND[iStrNodem1].d1tot);
843 
844  WM.Add(9 + 1, iReactionIndex + 1, dTmp1Wedge);
845 
846  /* termini del tipo: c*F/\*d/\*Deltag */
847  WM.Add(9 + 1, 3 + 1, FTmpWedge*dTmp1Wedge);
848 
849  /* termini di vincolo dovuti ai nodi */
850  WM.Sub(iReactionIndex + 1, 3 + 1, dTmp1Wedge);
851 
852  /* termini aggiuntivi dovuti alla deformabilita' */
853  SubMat3.RightMult(PHItT, RT*FTmpWedge);
854  WM.Add(iRigidOffset + NModes + 1, 3 + 1, SubMat3);
855 
856  SubMat1.LeftMult(FTmpWedge, PHItR);
857  WM.Sub(9 + 1, iRigidOffset + 1, SubMat1);
858  }
859 
860  /* contributo dovuto alla flessibilita' */
861  WM.Add(iReactionIndex + 1, iRigidOffset + 1, PHItR);
862 
863  /* idem per pd2 e pR2 */
864  Mat3x3 dTmp2Wedge(MatCross, SND[iStrNodem1].R2*SND[iStrNodem1].OffsetMB);
865  WM.Sub(iStrNodeIndex + 3 + 1, iReactionIndex + 1, dTmp2Wedge);
866 
867  /* termini del tipo: c*F/\*d/\*Deltag */
868  WM.Sub(iStrNodeIndex + 3 + 1, iStrNodeIndex + 3 + 1,
869  FTmpWedge*dTmp2Wedge);
870 
871  /* termini aggiuntivi dovuti alla deformabilita' */
872  SubMat3.RightMult(PHItT, RT);
873  WM.Add(iRigidOffset + NModes + 1, iReactionIndex + 1, SubMat3);
874 
875  /* modifica: divido le equazioni di vincolo per dCoef */
876 
877  /* termini di vincolo dovuti al nodo 2 */
878  WM.Add(iReactionIndex + 1, iStrNodeIndex + 3 + 1, dTmp2Wedge);
879 
880  /* fine cerniera sferica */
881 
882  /* equazioni di vincolo : giunto prismatico */
883 
884  /* Vec3 M(XCurr, iModalIndex + 2*NModes + 6*iStrNodem1 + 3 + 1); */
885  Vec3 MTmp = SND[iStrNodem1].R2*(SND[iStrNodem1].M*dCoef);
886  Mat3x3 MTmpWedge(MatCross, MTmp);
887 
888  /* Eq. dei momenti */
889  if (pModalNode) {
890  WM.Sub(9 + 1, iStrNodeIndex + 3 + 1, MTmpWedge);
891  }
892  WM.Add(iStrNodeIndex + 3 + 1, iStrNodeIndex + 3 + 1, MTmpWedge);
893 
894  /* Eq. d'equilibrio ai modi */
895  SubMat3.RightMult(PHIrT, R*MTmpWedge);
896  if (pModalNode) {
897  WM.Add(iRigidOffset + NModes + 1, 3 + 1, SubMat3);
898  }
899  WM.Sub(iRigidOffset + NModes + 1, iStrNodeIndex + 3 + 1, SubMat3);
900 
901  /* */
902  if (pModalNode) {
903  WM.AddT(iReactionIndex + 3 + 1, 3 + 1, SND[iStrNodem1].R2);
904  WM.Add(9 + 1, iReactionIndex + 3 + 1, SND[iStrNodem1].R2);
905  }
906  WM.SubT(iReactionIndex + 3 + 1, iStrNodeIndex + 3 + 1, SND[iStrNodem1].R2);
907  WM.Sub(iStrNodeIndex + 3 + 1, iReactionIndex + 3 + 1, SND[iStrNodem1].R2);
908 
909  /* */
910  SubMat3.RightMult(PHIrT, RT*SND[iStrNodem1].R2);
911  WM.Add(iRigidOffset + NModes + 1, iReactionIndex + 3 + 1, SubMat3);
912  for (unsigned iMode = 1; iMode <= NModes; iMode++) {
913  for (unsigned jIdx = 1; jIdx <= 3; jIdx++) {
914  WM.IncCoef(iReactionIndex + 3 + jIdx, iRigidOffset + iMode,
915  SubMat3(iMode, jIdx));
916  }
917  }
918  }
919 #endif
920  return WorkMat;
921 }
922 
923 SubVectorHandler&
924 Modal::AssRes(SubVectorHandler& WorkVec,
925  doublereal dCoef,
926  const VectorHandler& XCurr,
927  const VectorHandler& XPrimeCurr)
928 {
929  DEBUGCOUT("Entering Modal::AssRes()" << std::endl);
930 #if USE_AUTODIFF && MODAL_USE_AUTODIFF && !MODAL_DEBUG_AUTODIFF
931  grad::GradientAssVec<doublereal>::AssRes(this,
932  WorkVec,
933  dCoef,
934  XCurr,
935  XPrimeCurr,
936  grad::REGULAR_RES);
937 #else
938  integer iNumRows;
939  integer iNumCols;
940 
941  WorkSpaceDim(&iNumRows, &iNumCols);
942  WorkVec.ResizeReset(iNumRows);
943 
944 #if USE_AUTODIFF && MODAL_USE_AUTODIFF && MODAL_DEBUG_AUTODIFF
945  static integer iResidual = 0;
946  silent_cerr("Residual: #" << ++iResidual << std::endl);
947 
948  MySubVectorHandler WorkVecAD(iNumRows);
949  WorkVecAD.Resize(iNumRows);
950  WorkVecAD.Reset();
951  grad::GradientAssVec<doublereal>::AssRes(this,
952  WorkVecAD,
953  dCoef,
954  XCurr,
955  XPrimeCurr,
956  grad::REGULAR_RES);
957 #endif
958 
959  /*
960  * Equations:
961  *
962  * 1 -> 12: rigid body eq.
963  *
964  * 13 -> 12 + 2*NM: modes eq. (\dot{a}=b); m\dot{b} + ka=f)
965  *
966  * 12 + 2*NM -> 12 + 2*NM + 6*NN: node constraints
967  *
968  * 12 + 2*NM + 6*NN -> 12 + 2*NM + 6*NN + 6*NN: constr. nodes eq.
969  *
970  *
971  * Unknowns:
972  *
973  * rigid body: from modal node
974  * modes: iGetFirstIndex()
975  * node reactions: iGetFirstIndex() + 2*NM
976  * nodes: from constraint nodes
977  */
978 
979  /* modal dofs and node constraints indices */
980  integer iModalIndex = iGetFirstIndex();
981  unsigned int iNumDof = iGetNumDof();
982  for (unsigned int iCnt = 1; iCnt <= iNumDof; iCnt++) {
983  WorkVec.PutRowIndex(iRigidOffset + iCnt, iModalIndex + iCnt);
984  }
985 
986  /* interface nodes equilibrium indices */
987  integer iOffset = iRigidOffset + iNumDof;
988  for (unsigned iStrNodem1 = 0; iStrNodem1 < NStrNodes; iStrNodem1++) {
989  integer iNodeFirstMomIndex =
990  SND[iStrNodem1].pNode->iGetFirstMomentumIndex();
991 
992  for (unsigned int iCnt = 1; iCnt <= 6; iCnt++) {
993  WorkVec.PutRowIndex(iOffset + iCnt,
994  iNodeFirstMomIndex + iCnt);
995  }
996 
997  iOffset += 6;
998  }
999 
1000  a.Copy(XCurr, iModalIndex + 1);
1001  aPrime.Copy(XPrimeCurr, iModalIndex + 1);
1002  b.Copy(XCurr, iModalIndex + NModes + 1);
1003  bPrime.Copy(XPrimeCurr, iModalIndex + NModes + 1);
1004 
1005  VecN Ka(NModes), CaP(NModes), MaPP(NModes);
1006 
1007  /*
1008  * aggiornamento invarianti
1009  */
1010  Ka.Mult(*pModalStiff, a);
1011  CaP.Mult(*pModalDamp, b);
1012  MaPP.Mult(*pModalMass, bPrime);
1013 
1014 #if 0
1015  std::cerr << "### Stiff" << std::endl;
1016  for (unsigned int i = 1; i <= NModes; i++) {
1017  for (unsigned int j = 1; j <= NModes; j++) {
1018  std::cerr << std::setw(2) << i << ", "
1019  << std::setw(2) << j << " "
1020  << std::setw(20) << (*pModalStiff)(i,j)
1021  << std::endl;
1022 
1023  }
1024  }
1025  std::cerr << "### Damp" << std::endl;
1026  for (unsigned int i = 1; i <= NModes; i++) {
1027  for (unsigned int j = 1; j <= NModes; j++) {
1028  std::cerr << std::setw(2) << i << ", "
1029  << std::setw(2) << j << " "
1030  << std::setw(20) << (*pModalDamp)(i,j)
1031  << std::endl;
1032  }
1033  }
1034  std::cerr << "### Mass" << std::endl;
1035  for (unsigned int i = 1; i <= NModes; i++) {
1036  for (unsigned int j = 1; j <= NModes; j++) {
1037  std::cerr << std::setw(2) << i << ", "
1038  << std::setw(2) << j << " "
1039  << std::setw(20) << (*pModalMass)(i,j)
1040  << std::endl;
1041  }
1042  }
1043  std::cerr << "### a" << std::endl;
1044  for (unsigned int i = 1; i <= NModes; i++) {
1045  std::cerr << std::setw(2) << i << " "
1046  << std::setw(20) << a(i) << std::endl;
1047  }
1048  std::cerr << "### b" << std::endl;
1049  for (unsigned int i = 1; i <= NModes; i++) {
1050  std::cerr << std::setw(2) << i << " "
1051  << std::setw(20) << b(i) << std::endl;
1052  }
1053  std::cerr << "### bP" << std::endl;
1054  for (unsigned int i = 1; i <= NModes; i++) {
1055  std::cerr << std::setw(2) << i << " "
1056  << std::setw(20) << bPrime(i) << std::endl;
1057  }
1058 #endif
1059 
1060  Vec3 Inv3jaPPj(::Zero3);
1061  if (pInv3 != 0) {
1062  Inv3jaj = *pInv3 * a;
1063  Inv3jaPj = *pInv3 * b;
1064  Inv3jaPPj = *pInv3 * bPrime;
1065  }
1066 
1067  /* invarianti rotazionali */
1068  if (pInv8 != 0) {
1069  Inv8jaj.Reset();
1070  Inv8jaPj.Reset();
1071  }
1072  if (pInv5 != 0) {
1073  Inv5jaj.Reset(0.);
1074  Inv5jaPj.Reset(0.);
1075  }
1076 
1077  // Mat3x3 MatTmp1(::Zero3x3), MatTmp2(::Zero3x3);
1078 
1079  if (pInv9) {
1080  Inv9jkajak.Reset();
1081  Inv9jkajaPk.Reset();
1082  }
1083  Mat3x3 Inv10jaPj(::Zero3x3);
1084 
1085  if (pInv5 != 0 || pInv8 != 0 || pInv9 != 0 || pInv10 != 0) {
1086  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
1087  doublereal a_jMode = a(jMode);
1088  doublereal aP_jMode = b(jMode);
1089 
1090  if (pInv5 != 0) {
1091  for (unsigned int kMode = 1; kMode <= NModes; kMode++) {
1092  Vec3 v = pInv5->GetVec((jMode - 1)*NModes + kMode);
1093 
1094  Inv5jaj.AddVec(kMode, v*a_jMode);
1095  Inv5jaPj.AddVec(kMode, v*aP_jMode);
1096  }
1097  }
1098 
1099  if (pInv8 != 0) {
1100  Mat3x3 Inv8jajTmp;
1101 
1102  for (unsigned int jCnt = 1; jCnt <= 3; jCnt++) {
1103  unsigned int iMjC = (jMode - 1)*3 + jCnt;
1104 
1105  Inv8jajTmp.PutVec(jCnt, pInv8->GetVec(iMjC));
1106  }
1107 
1108  Inv8jaj += Inv8jajTmp * a_jMode;
1109  Inv8jaPj += Inv8jajTmp * aP_jMode;
1110 
1111  if (pInv9 != 0) {
1112  /*
1113  * questi termini si possono commentare perche' sono
1114  * (sempre ?) piccoli (termini del tipo a*a o a*b)
1115  * eventualmente dare all'utente la possibilita'
1116  * di scegliere se trascurarli o no
1117  */
1118  for (unsigned int kMode = 1; kMode <= NModes; kMode++) {
1119  doublereal a_kMode = a(kMode);
1120  doublereal aP_kMode = b(kMode);
1121  unsigned int iOffset = (jMode - 1)*3*NModes + (kMode - 1)*3 + 1;
1122  Inv9jkajak += pInv9->GetMat3x3ScalarMult(iOffset, a_jMode*a_kMode);
1123  Inv9jkajaPk += pInv9->GetMat3x3ScalarMult(iOffset, a_jMode*aP_kMode);
1124  }
1125  }
1126  }
1127 
1128  if (pInv10 != 0) {
1129  Mat3x3 Inv10jaPjTmp;
1130 
1131  for (unsigned int jCnt = 1; jCnt <= 3; jCnt++) {
1132  unsigned int iMjC = (jMode - 1)*3 + jCnt;
1133 
1134  Inv10jaPjTmp.PutVec(jCnt, pInv10->GetVec(iMjC)*aP_jMode);
1135  }
1136 
1137 
1138  Inv10jaPj += Inv10jaPjTmp;
1139  }
1140  }
1141  } /* fine ciclo sui modi */
1142 
1143 #ifdef MODAL_USE_GRAVITY
1144  /* forza di gravita' (decidere come inserire g) */
1145  /* FIXME: use a reasonable reference point where compute gravity */
1146  Vec3 GravityAcceleration(::Zero3);
1147  bool bGravity = GravityOwner::bGetGravity(x, GravityAcceleration);
1148 #endif /* MODAL_USE_GRAVITY */
1149 
1150  Vec3 vP(::Zero3);
1151  Vec3 wP(::Zero3);
1152  Vec3 w(::Zero3);
1153  Vec3 RTw(::Zero3);
1154  if (pModalNode) {
1155  /* rigid body indices */
1156  integer iRigidIndex = pModalNode->iGetFirstIndex();
1157  for (unsigned int iCnt = 1; iCnt <= iRigidOffset; iCnt++) {
1158  WorkVec.PutRowIndex(iCnt, iRigidIndex + iCnt);
1159  }
1160 
1161  /* recupera i dati necessari */
1162  x = pModalNode->GetXCurr();
1163  const Vec3& xP = pModalNode->GetVCurr();
1164  const Vec3& g = pModalNode->GetgCurr();
1165  const Vec3& gP = pModalNode->GetgPCurr();
1166  vP = pModalNode->GetXPPCurr();
1167  const Vec3& wr = pModalNode->GetWRef();
1168  wP = pModalNode->GetWPCurr();
1169 
1170  Vec3 v(XCurr, iRigidIndex + 6 + 1);
1171  w = Vec3(XCurr, iRigidIndex + 9 + 1);
1172 
1173  R = pModalNode->GetRCurr();
1174  RT = R.Transpose();
1175  RTw = RT*w;
1176 
1177  Mat3x3 JTmp = Inv7;
1178  if (pInv8 != 0) {
1179  JTmp += Inv8jaj.Symm2();
1180  if (pInv9 != 0) {
1181  JTmp -= Inv9jkajak;
1182  }
1183  }
1184  Mat3x3 J = R*JTmp*RT;
1185  Vec3 STmp = Inv2;
1186  if (pInv3 != 0) {
1187  STmp += Inv3jaj;
1188  }
1189  Vec3 S = R*STmp;
1190 
1191  /*
1192  * fine aggiornamento invarianti
1193  */
1194 
1195  /* forze d'inerzia */
1196  Vec3 FTmp = vP*dMass - S.Cross(wP) + w.Cross(w.Cross(S));
1197  if (pInv3 != 0) {
1198  FTmp += R*Inv3jaPPj + (w.Cross(R*Inv3jaPj))*2.;
1199  }
1200  WorkVec.Sub(6 + 1, FTmp);
1201 #if 0
1202  std::cerr << "m=" << dMass << "; S=" << S
1203  << "; a=" << a << "; aPrime =" << aPrime
1204  << "; b=" << b << "; bPrime= " << bPrime
1205  << "; tot=" << vP*dMass - S.Cross(wP) + w.Cross(w.Cross(S))
1206  + (w.Cross(R*Inv3jaPj))*2 + R*Inv3jaPPj << std::endl;
1207 #endif
1208 
1209  Vec3 MTmp = S.Cross(vP) + J*wP + w.Cross(J*w);
1210  if (pInv4 != 0) {
1211  Mat3xN Inv4Curr(NModes, 0);
1212  Inv4Curr.LeftMult(R, *pInv4);
1213  MTmp += Inv4Curr*bPrime;
1214  }
1215  if (pInv5 != 0) {
1216  Mat3xN Inv5jajCurr(NModes, 0);
1217  Inv5jajCurr.LeftMult(R, Inv5jaj);
1218  MTmp += Inv5jajCurr*bPrime;
1219  }
1220  if (pInv8 != 0) {
1221  Mat3x3 Tmp = Inv8jaPj;
1222  if (pInv9 != 0) {
1223  Tmp -= Inv9jkajaPk;
1224  }
1225  MTmp += R*Tmp*(RTw*2.);
1226  }
1227  /* termini dovuti alle inerzie rotazionali */
1228  if (pInv10 != 0) {
1229  Vec3 VTmp = Inv10jaPj.Symm2()*RTw;
1230  if (pInv11 != 0) {
1231  VTmp += w.Cross(R*(*pInv11*b));
1232  }
1233  MTmp += R*VTmp;
1234  }
1235  WorkVec.Sub(9 + 1, MTmp);
1236 
1237 #ifdef MODAL_USE_GRAVITY
1238  /* forza di gravita' (decidere come inserire g) */
1239  if (bGravity) {
1240  WorkVec.Add(6 + 1, GravityAcceleration*dMass);
1241  WorkVec.Add(9 + 1, S.Cross(GravityAcceleration));
1242  }
1243 #endif /* MODAL_USE_GRAVITY */
1244 
1245  /* equazioni per abbassare di grado il sistema */
1246  WorkVec.Add(1, v - xP);
1247  WorkVec.Add(3 + 1, w - Mat3x3(CGR_Rot::MatG, g)*gP
1248  - Mat3x3(CGR_Rot::MatR, g)*wr);
1249  }
1250 
1251  /* forze modali */
1252  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
1253  unsigned int jOffset = (jMode - 1)*3 + 1;
1254  doublereal d = - MaPP(jMode) - CaP(jMode) - Ka(jMode);
1255 
1256  if (pInv3 != 0) {
1257  Vec3 Inv3j = pInv3->GetVec(jMode);
1258 
1259  d -= (R*Inv3j).Dot(vP);
1260 
1261 #ifdef MODAL_USE_GRAVITY
1262  /* forza di gravita': */
1263  if (bGravity) {
1264  WorkVec.IncCoef(iRigidOffset + NModes + jMode,
1265  (R*Inv3j).Dot(GravityAcceleration));
1266  }
1267 #endif /* MODAL_USE_GRAVITY */
1268  }
1269 
1270  if (pInv4 != 0) {
1271  Vec3 Inv4j = pInv4->GetVec(jMode);
1272  if (pInv5 != 0) {
1273  Inv4j += Inv5jaj.GetVec(jMode);
1274 
1275  d -= ((R*Inv5jaPj.GetVec(jMode)).Dot(w))*2.;
1276  }
1277 
1278  d -= (R*Inv4j).Dot(wP);
1279  }
1280 
1281  if (pInv8 != 0 || pInv9 != 0 || pInv10 != 0) {
1282  Mat3x3 MTmp(::Zero3x3);
1283 
1284  if (pInv8 != 0) {
1285  MTmp += (pInv8->GetMat3x3(jOffset)).Transpose();
1286  if (pInv9 != 0) {
1287  for (unsigned int kModem1 = 0; kModem1 < NModes; kModem1++) {
1288  doublereal a_kMode = a(kModem1 + 1);
1289  integer kOffset = (jMode - 1)*3*NModes + kModem1*3 + 1;
1290 
1291  MTmp -= pInv9->GetMat3x3ScalarMult(kOffset, a_kMode);
1292  }
1293  }
1294  }
1295 
1296  if (pInv10 != 0) {
1297  MTmp += pInv10->GetMat3x3(jOffset);
1298  }
1299 
1300  d += w.Dot(R*(MTmp*RTw));
1301  }
1302 
1303  WorkVec.IncCoef(iRigidOffset + NModes + jMode, d);
1304 
1305 #if 0
1306  std::cerr << "(R*Inv3j).Dot(vP)=" << (R*Inv3j).Dot(vP)
1307  << std::endl
1308  << "(R*(Inv4j + VInv5jaj)).Dot(wP)="
1309  << (R*(Inv4j + VInv5jaj)).Dot(wP) << std::endl
1310  << "w.Dot(R*((Inv8jTranspose + Inv10j)*RTw))"
1311  << w.Dot(R*((Inv8jTranspose + Inv10j)*RTw)) << std::endl
1312  << " -(R*VInv5jaPj).Dot(w)*2."
1313  << -(R*VInv5jaPj).Dot(w)*2.<< std::endl
1314  << " -CaP.dGet(jMode)" << -CaP.dGet(jMode)<< std::endl
1315  << "-Ka.dGet(jMode) "
1316  << -CaP.dGet(jMode) - Ka.dGet(jMode) << std::endl;
1317 #endif
1318  }
1319 
1320  /* equazioni per abbassare di grado il sistema */
1321  for (unsigned int iCnt = 1; iCnt <= NModes; iCnt++) {
1322  WorkVec.PutCoef(iRigidOffset + iCnt, b(iCnt) - aPrime(iCnt));
1323  }
1324 
1325  /* equazioni di vincolo */
1326  for (unsigned int iStrNode = 1; iStrNode <= NStrNodes; iStrNode++) {
1327  unsigned int iStrNodem1 = iStrNode - 1;
1328  integer iReactionIndex = iRigidOffset + 2*NModes + 6*iStrNodem1;
1329  integer iStrNodeIndex = iReactionIndex + 6*NStrNodes;
1330 
1331  /* recupero le forme modali del nodo vincolato */
1332  /* FIXME: what about using Blitz++ ? :) */
1333  Mat3xN PHIt(NModes), PHIr(NModes);
1334  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
1335  integer iOffset = (jMode - 1)*NStrNodes + iStrNode;
1336  PHIt.PutVec(jMode, pPHIt->GetVec(iOffset));
1337  PHIr.PutVec(jMode, pPHIr->GetVec(iOffset));
1338  }
1339 
1340  /*
1341  * aggiorno d1 e R con il contributo dovuto
1342  * alla flessibilita':
1343  * d1tot = R*[d1 + PHIt*a], R1tot = R*[I + (PHIr*a)/\]
1344  */
1345  SND[iStrNodem1].d1tot = R*(SND[iStrNodem1].OffsetFEM + PHIt*a);
1346  SND[iStrNodem1].R1tot = R*Mat3x3(1., PHIr*a);
1347 
1348  /* constraint reaction (force) */
1349  SND[iStrNodem1].F = Vec3(XCurr,
1350  iModalIndex + 2*NModes + 6*iStrNodem1 + 1);
1351  const Vec3& x2 = SND[iStrNodem1].pNode->GetXCurr();
1352 
1353  /* FIXME: R2??? */
1354  SND[iStrNodem1].R2 = SND[iStrNodem1].pNode->GetRCurr();
1355 
1356  /* cerniera sferica */
1357  Vec3 dTmp1(SND[iStrNodem1].d1tot);
1358  Vec3 dTmp2(SND[iStrNodem1].R2*SND[iStrNodem1].OffsetMB);
1359 
1360  /* Eq. d'equilibrio, nodo 1 */
1361  if (pModalNode) {
1362  WorkVec.Sub(6 + 1, SND[iStrNodem1].F);
1363  WorkVec.Sub(9 + 1, dTmp1.Cross(SND[iStrNodem1].F));
1364  }
1365 
1366  /* termine aggiuntivo dovuto alla deformabilita':
1367  * -PHItiT*RT*F */
1368  Vec3 vtemp = RT*SND[iStrNodem1].F;
1369  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
1370  /* PHItTranspose(jMode) * vtemp */
1371  WorkVec.DecCoef(iRigidOffset + NModes + jMode,
1372  PHIt.GetVec(jMode)*vtemp);
1373  }
1374 
1375  /* Eq. d'equilibrio, nodo 2 */
1376  WorkVec.Add(iStrNodeIndex + 1, SND[iStrNodem1].F);
1377  WorkVec.Add(iStrNodeIndex + 3 + 1, dTmp2.Cross(SND[iStrNodem1].F));
1378 
1379  /* Eq. di vincolo */
1380  ASSERT(dCoef != 0.);
1381  WorkVec.Add(iReactionIndex + 1, (x2 + dTmp2 - x - dTmp1)/dCoef);
1382 
1383  /* constraint reaction (moment) */
1384  SND[iStrNodem1].M = Vec3(XCurr,
1385  iModalIndex + 2*NModes + 6*iStrNodem1 + 3 + 1);
1386 
1387  /* giunto prismatico */
1388  Vec3 ThetaCurr = RotManip::VecRot(SND[iStrNodem1].R2.MulTM(SND[iStrNodem1].R1tot));
1389  Vec3 MTmp = SND[iStrNodem1].R2*SND[iStrNodem1].M;
1390 
1391  /* Equazioni di equilibrio, nodo 1 */
1392  if (pModalNode) {
1393  WorkVec.Sub(9 + 1, MTmp);
1394  }
1395 
1396  /* Equazioni di equilibrio, nodo 2 */
1397  WorkVec.Add(iStrNodeIndex + 3 + 1, MTmp);
1398 
1399  /* Contributo dovuto alla flessibilita' :-PHIrT*RtotT*M */
1400  vtemp = RT*MTmp;
1401  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
1402  /* PHIrTranspose(jMode) * vtemp */
1403  WorkVec.DecCoef(iRigidOffset + NModes + jMode,
1404  PHIr.GetVec(jMode)*vtemp);
1405  }
1406 
1407  /* Modifica: divido le equazioni di vincolo per dCoef */
1408  ASSERT(dCoef != 0.);
1409  /* Equazioni di vincolo di rotazione */
1410  WorkVec.Sub(iReactionIndex + 3 + 1, ThetaCurr/dCoef);
1411  }
1412 
1413 #if 0
1414  std::cerr << "###" << std::endl;
1415  for (int i = 1; i <= WorkVec.iGetSize(); i++) {
1416  std::cerr << std::setw(2) << i << "("
1417  << std::setw(2) << WorkVec.iGetRowIndex(i) << ")"
1418  << std::setw(20) << WorkVec(i) << std::endl;
1419  }
1420 #endif
1421 #endif
1422 
1423 #if USE_AUTODIFF && MODAL_USE_AUTODIFF && MODAL_DEBUG_AUTODIFF
1424  doublereal dTol = 1.;
1425 
1426  for (integer i = 1; i <= WorkVec.iGetSize(); ++i) {
1427  dTol = std::max(dTol, fabs(WorkVec.dGetCoef(i)));
1428  }
1429 
1430  dTol *= sqrt(std::numeric_limits<doublereal>::epsilon());
1431  bool bOK = true;
1432  for (integer i = 1; i <= WorkVec.iGetSize(); ++i) {
1433  const doublereal dVal = WorkVec.dGetCoef(i);
1434  const integer iRow = WorkVec.iGetRowIndex(i);
1435  doublereal dValAD = 0.;
1436 
1437  for (integer j = 1; j <= WorkVecAD.iGetSize(); ++j) {
1438  if (iRow == WorkVecAD.iGetRowIndex(j)) {
1439  dValAD += WorkVecAD.dGetCoef(j);
1440  }
1441  }
1442 
1443  if (fabs(dVal - dValAD) > dTol) {
1444  silent_cerr("\tRowIndex: " << iRow << " WorkVec(" << i << ")=" << dVal << ", WorkVecAD(" << i << ")=" << dValAD << std::endl);
1445  bOK = false;
1446  }
1447  }
1448  if (!bOK) {
1449  silent_cerr("Residual #" << iResidual << ": NOK" << std::endl);
1450  }
1451  GRADIENT_ASSERT(bOK);
1452 #endif
1453  return WorkVec;
1454 }
1455 
1456 #if USE_AUTODIFF && MODAL_USE_AUTODIFF
1457 void
1458 Modal::UpdateStrNodeData(Modal::StrNodeData& oNode,
1459  const grad::Vector<doublereal, 3>& d1tot,
1460  const grad::Matrix<doublereal, 3, 3>& R1tot,
1461  const grad::Vector<doublereal, 3>& F,
1462  const grad::Vector<doublereal, 3>& M,
1463  const grad::Matrix<doublereal, 3, 3>& R2)
1464 {
1465  using namespace grad;
1466 
1467  for (index_type i = 1; i <= 3; ++i) {
1468  oNode.d1tot(i) = d1tot(i);
1469  oNode.F(i) = F(i);
1470  oNode.M(i) = M(i);
1471  }
1472 
1473  for (index_type j = 1; j <= 3; ++j) {
1474  for (index_type i = 1; i <= 3; ++i) {
1475  oNode.R1tot(i, j) = R1tot(i, j);
1476  oNode.R2(i, j) = R2(i, j);
1477  }
1478  }
1479 }
1480 
1481 void
1482 Modal::UpdateModalNode(const grad::Vector<doublereal, 3>& xTmp,
1483  const grad::Matrix<doublereal, 3, 3>& RTmp)
1484 {
1485  using namespace grad;
1486 
1487  for (index_type i = 1; i <= 3; ++i) {
1488  x(i) = xTmp(i);
1489  }
1490 
1491  for (index_type j = 1; j <= 3; ++j) {
1492  for (index_type i = 1; i <= 3; ++i) {
1493  R(i, j) = RTmp(i, j);
1494  }
1495  }
1496 }
1497 
1498 void
1499 Modal::UpdateState(const grad::Vector<doublereal, grad::DYNAMIC_SIZE>& aTmp,
1500  const grad::Vector<doublereal, grad::DYNAMIC_SIZE>& aPrimeTmp,
1501  const grad::Vector<doublereal, grad::DYNAMIC_SIZE>& bTmp,
1502  const grad::Vector<doublereal, grad::DYNAMIC_SIZE>& bPrimeTmp)
1503 {
1504  using namespace grad;
1505 
1506  for (index_type i = 1; i <= a.iGetNumRows(); ++i) {
1507  a(i) = aTmp(i);
1508  }
1509 
1510  for (index_type i = 1; i <= aPrime.iGetNumRows(); ++i) {
1511  aPrime(i) = aPrimeTmp(i);
1512  }
1513 
1514  for (index_type i = 1; i <= b.iGetNumRows(); ++i) {
1515  b(i) = bTmp(i);
1516  }
1517 
1518  for (index_type i = 1; i <= bPrime.iGetNumRows(); ++i) {
1519  bPrime(i) = bPrimeTmp(i);
1520  }
1521 }
1522 
1523 void
1524 Modal::UpdateInvariants(const grad::Vector<doublereal, 3>& Inv3jajTmp,
1525  const grad::Matrix<doublereal, 3, 3>& Inv8jajTmp,
1526  const grad::Matrix<doublereal, 3, 3>& Inv9jkajakTmp)
1527 {
1528  using namespace grad;
1529 
1530  for (index_type i = 1; i <= Inv3jajTmp.iGetNumRows(); ++i) {
1531  Inv3jaj(i) = Inv3jajTmp(i);
1532  }
1533 
1534  for (index_type j = 1; j <= Inv8jajTmp.iGetNumCols(); ++j) {
1535  for (index_type i = 1; i <= Inv8jajTmp.iGetNumRows(); ++i) {
1536  Inv8jaj(i, j) = Inv8jajTmp(i, j);
1537  }
1538  }
1539 
1540  for (index_type j = 1; j <= Inv9jkajakTmp.iGetNumCols(); ++j) {
1541  for (index_type i = 1; i <= Inv9jkajakTmp.iGetNumRows(); ++i) {
1542  Inv9jkajak(i, j) = Inv9jkajakTmp(i, j);
1543  }
1544  }
1545 }
1546 
1547 template <typename T>
1548 void
1549 Modal::AssRes(grad::GradientAssVec<T>& WorkVec,
1550  const doublereal dCoef,
1551  const grad::GradientVectorHandler<T>& XCurr,
1552  const grad::GradientVectorHandler<T>& XPrimeCurr,
1553  const grad::FunctionCall func)
1554 {
1555  using namespace grad;
1556 
1557  const integer iModalIndex = iGetFirstIndex();
1558 
1559  typedef Matrix<T, 3, 3> Mat3x3;
1560  typedef Vector<T, 3> Vec3;
1561  typedef Vector<T, DYNAMIC_SIZE> VecN;
1562  typedef Matrix<T, 3, DYNAMIC_SIZE> Mat3xN;
1563 
1564  VecN a(NModes), aPrime(NModes), b(NModes), bPrime(NModes);
1565 
1566  for (integer i = 1; i <= NModes; ++i) {
1567  XCurr.dGetCoef(iModalIndex + i, a(i), dCoef, &dofMap);
1568  XPrimeCurr.dGetCoef(iModalIndex + i, aPrime(i), 1., &dofMap);
1569  }
1570 
1571  for (integer i = 1; i <= NModes; ++i) {
1572  XCurr.dGetCoef(iModalIndex + NModes + i, b(i), dCoef, &dofMap);
1573  XPrimeCurr.dGetCoef(iModalIndex + NModes + i, bPrime(i), 1., &dofMap);
1574  }
1575 
1576  const VecN Ka = *pModalStiff * a;
1577  const VecN CaP = *pModalDamp * b;
1578  const VecN MaPP = *pModalMass * bPrime;
1579 
1580  Vec3 Inv3jaj, Inv3jaPj, Inv3jaPPj;
1581 
1582  if (pInv3) {
1583  Inv3jaj = *pInv3 * a;
1584  Inv3jaPj = *pInv3 * b;
1585  Inv3jaPPj = *pInv3 * bPrime;
1586  }
1587 
1588  Mat3x3 Inv8jaj, Inv8jaPj;
1589  Mat3xN Inv5jaj(3, NModes), Inv5jaPj(3, NModes);
1590  Mat3x3 Inv9jkajak, Inv9jkajaPk;
1591  Mat3x3 Inv10jaPj;
1592 
1593  if (pInv5 || pInv8 || pInv9 || pInv10) {
1594  for (index_type jMode = 1; jMode <= NModes; jMode++) {
1595  const T& a_jMode = a(jMode);
1596  const T& aP_jMode = b(jMode);
1597 
1598  if (pInv5) {
1599  for (index_type kMode = 1; kMode <= NModes; kMode++) {
1600  for (index_type i = 1; i <= 3; ++i) {
1601  const doublereal v_i = (*pInv5)(i, (jMode - 1) * NModes + kMode);
1602  Inv5jaj(i, kMode) += v_i * a_jMode;
1603  Inv5jaPj(i, kMode) += v_i * aP_jMode;
1604  }
1605  }
1606  }
1607 
1608  if (pInv8) {
1609  for (index_type jCnt = 1; jCnt <= 3; jCnt++) {
1610  const index_type iMjC = (jMode - 1) * 3 + jCnt;
1611  for (index_type i = 1; i <= 3; ++i) {
1612  Inv8jaj(i, jCnt) += (*pInv8)(i, iMjC) * a_jMode;
1613  Inv8jaPj(i, jCnt) += (*pInv8)(i, iMjC) * aP_jMode;
1614  }
1615  }
1616 
1617  if (pInv9) {
1618  for (index_type kMode = 1; kMode <= NModes; kMode++) {
1619  const T& a_kMode = a(kMode);
1620  const T& aP_kMode = b(kMode);
1621  const index_type iOffset = (jMode - 1) * 3 * NModes + (kMode - 1) * 3;
1622 
1623  for (index_type i = 1; i <= 3; ++i) {
1624  for (index_type j = 1; j <= 3; ++j) {
1625  Inv9jkajak(i, j) += (*pInv9)(i, iOffset + j) * a_jMode * a_kMode;
1626  Inv9jkajaPk(i, j) += (*pInv9)(i, iOffset + j) * a_jMode * aP_kMode;
1627  }
1628  }
1629  }
1630  }
1631  }
1632 
1633  if (pInv10) {
1634  for (index_type jCnt = 1; jCnt <= 3; jCnt++) {
1635  const index_type iMjC = (jMode - 1) * 3 + jCnt;
1636 
1637  for (index_type i = 1; i <= 3; ++i) {
1638  Inv10jaPj(i, jCnt) += (*pInv10)(i, iMjC) * aP_jMode;
1639  }
1640  }
1641  }
1642  }
1643  }
1644 
1645 #ifdef MODAL_USE_GRAVITY
1646  /* forza di gravita' (decidere come inserire g) */
1647  /* FIXME: use a reasonable reference point where compute gravity */
1648  ::Vec3 GravityAcceleration(::Zero3);
1649  const bool bGravity = GravityOwner::bGetGravity(this->x, GravityAcceleration);
1650 #endif /* MODAL_USE_GRAVITY */
1651 
1652  const integer iRigidIndex = pModalNode ? pModalNode->iGetFirstIndex() : -1;
1653 
1654  Vec3 x(this->x), vP, wP, w, RTw;
1655  Mat3x3 R(this->R);
1656  Vec3 FTmp, MTmp;
1657 
1658  if (pModalNode) {
1659  Vec3 xP, g, gP, v;
1660 
1661  pModalNode->GetXCurr(x, dCoef, func, &dofMap);
1662  pModalNode->GetVCurr(xP, dCoef, func, &dofMap);
1663  pModalNode->GetgCurr(g, dCoef, func, &dofMap);
1664  pModalNode->GetgPCurr(gP, dCoef, func, &dofMap);
1665  pModalNode->GetXPPCurr(vP, dCoef, func, &dofMap);
1666  const ::Vec3& wr = pModalNode->GetWRef();
1667  pModalNode->GetWPCurr(wP, dCoef, func, &dofMap);
1668 
1669  for (index_type i = 1; i <= 3; ++i) {
1670  XCurr.dGetCoef(iRigidIndex + 6 + i, v(i), dCoef, &dofMap);
1671  XCurr.dGetCoef(iRigidIndex + 9 + i, w(i), dCoef, &dofMap);
1672  }
1673 
1674  pModalNode->GetRCurr(R, dCoef, func, &dofMap);
1675 
1676  RTw = Transpose(R) * w;
1677 
1678  Mat3x3 J(Inv7);
1679 
1680  if (pInv8) {
1681  J += Inv8jaj + Transpose(Inv8jaj);
1682  if (pInv9 != 0) {
1683  J -= Inv9jkajak;
1684  }
1685  }
1686 
1687  J = Mat3x3(R * J) * Transpose(R);
1688 
1689  Vec3 STmp(Inv2);
1690 
1691  if (pInv3) {
1692  STmp += Inv3jaj;
1693  }
1694 
1695  const Vec3 S = R * STmp;
1696 
1697  FTmp = vP * -dMass + Cross(S, wP) - Cross(w, Vec3(Cross(w, S)));
1698 
1699  if (pInv3 != 0) {
1700  FTmp -= R * Inv3jaPPj + Cross(w, Vec3(R * Inv3jaPj)) * 2.;
1701  }
1702 
1703 #ifdef MODAL_USE_GRAVITY
1704  if (bGravity) {
1705  FTmp += GravityAcceleration * dMass;
1706  }
1707 #endif /* MODAL_USE_GRAVITY */
1708 
1709  MTmp = -Cross(S, vP) - J * wP - Cross(w, Vec3(J * w));
1710 
1711  if (pInv4) {
1712  MTmp -= R * Vec3((*pInv4) * bPrime);
1713  }
1714 
1715  if (pInv5) {
1716  MTmp -= R * Vec3(Inv5jaj * bPrime);
1717  }
1718 
1719  if (pInv8) {
1720  Mat3x3 Tmp = Inv8jaPj;
1721  if (pInv9 != 0) {
1722  Tmp -= Inv9jkajaPk;
1723  }
1724  MTmp -= R * Vec3(Tmp * RTw * 2.);
1725  }
1726 
1727  if (pInv10) {
1728  Vec3 VTmp = Mat3x3(Inv10jaPj + Transpose(Inv10jaPj)) * RTw;
1729  if (pInv11) {
1730  VTmp += Cross(w, Vec3(R * Vec3(*pInv11 * b)));
1731  }
1732  MTmp -= R * VTmp;
1733  }
1734 
1735 #ifdef MODAL_USE_GRAVITY
1736  if (bGravity) {
1737  MTmp += Cross(S, GravityAcceleration);
1738  }
1739 #endif
1740  const Vec3 f1 = v - xP;
1741  const Vec3 f2 = w - Mat3x3(MatGVec(g)) * gP - Mat3x3(MatRVec(g)) * wr;
1742 
1743  WorkVec.AddItem(iRigidIndex + 1, f1);
1744  WorkVec.AddItem(iRigidIndex + 4, f2);
1745  }
1746 
1747  for (index_type jMode = 1; jMode <= NModes; jMode++) {
1748  index_type jOffset = (jMode - 1) * 3 + 1;
1749  T d = -MaPP(jMode) - CaP(jMode) - Ka(jMode);
1750 
1751  if (pInv3) {
1752  const Vec3 RInv3j = R * pInv3->GetVec(jMode);
1753 
1754  d -= Dot(RInv3j, vP);
1755 
1756 #ifdef MODAL_USE_GRAVITY
1757  if (bGravity) {
1758  d += Dot(RInv3j, GravityAcceleration);
1759  }
1760 #endif /* MODAL_USE_GRAVITY */
1761  }
1762 
1763  if (pInv4) {
1764  Vec3 Inv4j(pInv4->GetVec(jMode));
1765  if (pInv5) {
1766  Inv4j += Inv5jaj.GetCol(jMode);
1767  d -= Dot(R * Inv5jaPj.GetCol(jMode), w) * 2.;
1768  }
1769 
1770  d -= Dot(R * Inv4j, wP);
1771  }
1772 
1773  if (pInv8 || pInv9 || pInv10) {
1774  Mat3x3 MatTmp;
1775 
1776  if (pInv8) {
1777  for (index_type i = 1; i <= 3; ++i) {
1778  for (index_type j = 1; j <= 3; ++j) {
1779  MatTmp(j, i) += (*pInv8)(i, jOffset + j - 1);
1780  }
1781  }
1782  if (pInv9) {
1783  for (index_type kModem1 = 0; kModem1 < NModes; kModem1++) {
1784  const T& a_kMode = a(kModem1 + 1);
1785  const index_type kOffset = (jMode - 1) * 3 * NModes + kModem1 * 3 + 1;
1786 
1787  for (index_type i = 1; i <= 3; ++i) {
1788  for (index_type j = 1; j <= 3; ++j) {
1789  MatTmp(i, j) -= (*pInv9)(i, kOffset + j - 1) * a_kMode;
1790  }
1791  }
1792  }
1793  }
1794  }
1795 
1796  if (pInv10) {
1797  for (index_type i = 1; i <= 3; ++i) {
1798  for (index_type j = 1; j <= 3; ++j) {
1799  MatTmp(i, j) += (*pInv10)(i, jOffset + j - 1);
1800  }
1801  }
1802  }
1803 
1804  d += Dot(w, R * Vec3(MatTmp * RTw));
1805  }
1806 
1807  WorkVec.AddItem(iModalIndex + NModes + jMode, d);
1808  }
1809 
1810  for (index_type iCnt = 1; iCnt <= NModes; iCnt++) {
1811  WorkVec.AddItem(iModalIndex + iCnt, b(iCnt) - aPrime(iCnt));
1812  }
1813 
1814  for (index_type iStrNode = 1; iStrNode <= NStrNodes; iStrNode++) {
1815  const index_type iStrNodem1 = iStrNode - 1;
1816  const integer iNodeFirstMomIndex = SND[iStrNodem1].pNode->iGetFirstMomentumIndex();
1817 
1818  Vec3 PHIta, PHIra;
1819 
1820  for (index_type jMode = 1; jMode <= NModes; jMode++) {
1821  const index_type iOffset = (jMode - 1) * NStrNodes + iStrNode;
1822  for (index_type i = 1; i <= 3; ++i) {
1823  PHIta(i) += (*pPHIt)(i, iOffset) * a(jMode);
1824  PHIra(i) += (*pPHIr)(i, iOffset) * a(jMode);
1825  }
1826  }
1827 
1828  const Vec3 d1tot = R * Vec3(PHIta + SND[iStrNodem1].OffsetFEM);
1829  const Mat3x3 R1tot = R * Mat3x3(MatCrossVec(PHIra, 1.));
1830 
1831  Vec3 F;
1832 
1833  for (index_type i = 1; i <= 3; ++i) {
1834  XCurr.dGetCoef(iModalIndex + 2 * NModes + 6 * iStrNodem1 + i, F(i), 1., &dofMap);
1835  }
1836 
1837  Vec3 x2;
1838 
1839  SND[iStrNodem1].pNode->GetXCurr(x2, dCoef, func, &dofMap);
1840 
1841  Mat3x3 R2;
1842 
1843  SND[iStrNodem1].pNode->GetRCurr(R2, dCoef, func, &dofMap);
1844 
1845  Vec3 dTmp2(R2 * SND[iStrNodem1].OffsetMB);
1846 
1847  if (pModalNode) {
1848  FTmp -= F;
1849  MTmp -= Cross(d1tot, F);
1850  }
1851 
1852  Vec3 vtemp = Transpose(R) * F;
1853 
1854  for (index_type jMode = 1; jMode <= NModes; jMode++) {
1855  const index_type iOffset = (jMode - 1) * NStrNodes + iStrNode;
1856  const T d = -Dot(vtemp, pPHIt->GetVec(iOffset));
1857  WorkVec.AddItem(iModalIndex + NModes + jMode, d);
1858  }
1859 
1860  Vec3 MTmp2 = Cross(dTmp2, F);
1861  Vec3 M;
1862 
1863  for (index_type i = 1; i <= 3; ++i) {
1864  XCurr.dGetCoef(iModalIndex + 2 * NModes + 6 * iStrNodem1 + 3 + i, M(i), 1., &dofMap);
1865  }
1866 
1867  const Mat3x3 DeltaR = Transpose(R2) * R1tot;
1868  const Vec3 ThetaCurr(VecRotMat(DeltaR));
1869  const Vec3 R2_M = R2 * M;
1870 
1871  if (pModalNode) {
1872  MTmp -= R2_M;
1873  }
1874 
1875  MTmp2 += R2_M; // Bug here
1876 
1877  vtemp = Transpose(R) * R2_M;
1878 
1879  for (index_type jMode = 1; jMode <= NModes; jMode++) {
1880  const index_type iOffset = (jMode - 1) * NStrNodes + iStrNode;
1881  const T d = -Dot(vtemp, pPHIr->GetVec(iOffset));
1882  WorkVec.AddItem(iModalIndex + NModes + jMode, d);
1883  }
1884 
1885  ASSERT(dCoef != 0.);
1886 
1887  const Vec3 f1 = (x2 + dTmp2 - x - d1tot) / dCoef;
1888  const Vec3 f2 = ThetaCurr / -dCoef;
1889 
1890  WorkVec.AddItem(iModalIndex + 2 * NModes + 6 * iStrNodem1 + 1, f1);
1891  WorkVec.AddItem(iModalIndex + 2 * NModes + 6 * iStrNodem1 + 4, f2);
1892 
1893  WorkVec.AddItem(iNodeFirstMomIndex + 1, F);
1894  WorkVec.AddItem(iNodeFirstMomIndex + 4, MTmp2); // Bug is here
1895 
1896  UpdateStrNodeData(SND[iStrNodem1], d1tot, R1tot, F, M, R2);
1897  }
1898 
1899  if (pModalNode) {
1900  WorkVec.AddItem(iRigidIndex + 7, FTmp);
1901  WorkVec.AddItem(iRigidIndex + 10, MTmp);
1902  }
1903 
1904  if (pModalNode) {
1905  UpdateModalNode(x, R);
1906  }
1907 
1908  UpdateState(a, aPrime, b, bPrime);
1909  UpdateInvariants(Inv3jaj, Inv8jaj, Inv9jkajak);
1910 }
1911 #endif
1912 
1913 void
1914 Modal::Output(OutputHandler& OH) const
1915 {
1916  if (bToBeOutput()) {
1917  /* stampa sul file di output i modi */
1918  std::ostream& out = OH.Modal();
1919 
1920  for (unsigned int iCnt = 1; iCnt <= NModes; iCnt++) {
1921  out << std::setw(8) << GetLabel() << '.' << uModeNumber[iCnt - 1]
1922  << " " << a(iCnt)
1923  << " " << b(iCnt)
1924  << " " << bPrime(iCnt)
1925  << std::endl;
1926  }
1927 
1928  std::vector<StrNodeData>::const_iterator i = SND.begin();
1929  std::vector<StrNodeData>::const_iterator end = SND.end();
1930  for (; i != end; ++i) {
1931  if (i->bOut) {
1932  out << std::setw(8) << GetLabel() << "@" << i->pNode->GetLabel()
1933  << " ", i->F.Write(out, " ")
1934  << " ", i->M.Write(out, " ")
1935  << std::endl;
1936  }
1937  }
1938  }
1939 }
1940 
1941 unsigned int
1942 Modal::iGetInitialNumDof(void) const
1943 {
1944  return 2*NModes + 12*NStrNodes;
1945 }
1946 
1947 void
1948 Modal::InitialWorkSpaceDim(integer* piNumRows, integer* piNumCols) const
1949 {
1950  *piNumRows = *piNumCols = iRigidOffset + iGetInitialNumDof() + 12*NStrNodes;
1951 }
1952 
1953 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
1954 VariableSubMatrixHandler&
1955 Modal::InitialAssJac(VariableSubMatrixHandler& WorkMat,
1956  const VectorHandler& XCurr)
1957 {
1958  DEBUGCOUT("Entering Modal::InitialAssJac()" << std::endl);
1959 
1960  FullSubMatrixHandler& WM = WorkMat.SetFull();
1961  integer iNumRows = 0;
1962  integer iNumCols = 0;
1963  InitialWorkSpaceDim(&iNumRows, &iNumCols);
1964  WM.ResizeReset(iNumRows, iNumCols);
1965 
1966  if (pModalNode) {
1967  integer iRigidIndex = pModalNode->iGetFirstIndex();
1968 
1969  for (unsigned int iCnt = 1; iCnt <= iRigidOffset; iCnt++) {
1970  WM.PutRowIndex(iCnt, iRigidIndex + iCnt);
1971  WM.PutColIndex(iCnt, iRigidIndex + iCnt);
1972  }
1973  }
1974 
1975  integer iFlexIndex = iGetFirstIndex();
1976 
1977  for (unsigned int iCnt = 1; iCnt <= iGetInitialNumDof(); iCnt++) {
1978  WM.PutRowIndex(iRigidOffset + iCnt, iFlexIndex + iCnt);
1979  WM.PutColIndex(iRigidOffset + iCnt, iFlexIndex + iCnt);
1980  }
1981 
1982  for (unsigned iStrNodem1 = 0; iStrNodem1 < NStrNodes; iStrNodem1++) {
1983  integer iNodeFirstPosIndex =
1984  SND[iStrNodem1].pNode->iGetFirstPositionIndex();
1985  integer iNodeFirstVelIndex = iNodeFirstPosIndex + 6;
1986 
1987  integer iOffset = iRigidOffset + iGetInitialNumDof() + 12*iStrNodem1;
1988  for (unsigned int iCnt = 1; iCnt <= 6; iCnt++) {
1989  WM.PutRowIndex(iOffset + iCnt,
1990  iNodeFirstPosIndex + iCnt);
1991  WM.PutColIndex(iOffset + iCnt,
1992  iNodeFirstPosIndex + iCnt);
1993  WM.PutRowIndex(iOffset + 6 + iCnt,
1994  iNodeFirstVelIndex + iCnt);
1995  WM.PutColIndex(iOffset + 6 + iCnt,
1996  iNodeFirstVelIndex + iCnt);
1997  }
1998  }
1999 
2000  /* comincia l'assemblaggio dello Jacobiano */
2001 
2002  /* nota: nelle prime iRigidOffset + 2*M equazioni
2003  * metto dei termini piccoli per evitare singolarita'
2004  * quando non ho vincoli esterni o ho azzerato i modi */
2005  for (unsigned int iCnt = 1; iCnt <= iRigidOffset + 2*NModes; iCnt++) {
2006  WM.PutCoef(iCnt, iCnt, 1.e-12);
2007  }
2008 
2009  /* forze elastiche e viscose */
2010  for (unsigned int iCnt = 1; iCnt <= NModes; iCnt++) {
2011  for (unsigned int jCnt = 1; jCnt <= NModes; jCnt++) {
2012  WM.PutCoef(iRigidOffset + iCnt, iRigidOffset + jCnt,
2013  pModalStiff->dGet(iCnt, jCnt));
2014  WM.PutCoef(iRigidOffset + iCnt, iRigidOffset + NModes + jCnt,
2015  pModalDamp->dGet(iCnt, jCnt));
2016  }
2017  }
2018 
2019  /* equazioni di vincolo */
2020 
2021  /* cerniera sferica */
2022  for (unsigned int iStrNode = 1; iStrNode <= NStrNodes; iStrNode++) {
2023  unsigned int iStrNodem1 = iStrNode - 1;
2024 
2025  /* recupera i dati */
2026  const Mat3x3& R2(SND[iStrNodem1].pNode->GetRRef());
2027  Vec3 Omega1(::Zero3);
2028  if (pModalNode) {
2029  Omega1 = pModalNode->GetWRef();
2030  }
2031  const Vec3& Omega2(SND[iStrNodem1].pNode->GetWRef());
2032  Vec3 F(XCurr, iFlexIndex + 2*NModes + 12*iStrNodem1 + 1);
2033  Vec3 FPrime(XCurr, iFlexIndex + 2*NModes + 12*iStrNodem1 + 6 + 1);
2034 
2035  Mat3xN PHIt(NModes, 0), PHIr(NModes, 0);
2036  MatNx3 PHItT(NModes, 0), PHIrT(NModes, 0);
2037 
2038  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
2039  PHIt.PutVec(jMode, pPHIt->GetVec((jMode - 1)*NStrNodes + iStrNode));
2040  PHIr.PutVec(jMode, pPHIr->GetVec((jMode - 1)*NStrNodes + iStrNode));
2041  }
2042 
2043  PHItT.Transpose(PHIt);
2044  PHIrT.Transpose(PHIr);
2045 
2046  Vec3 d1tot = R*(SND[iStrNodem1].OffsetFEM + PHIt*a);
2047  Mat3x3 R1tot = R*Mat3x3(1., PHIr*a);
2048  Mat3xN SubMat1(NModes, 0.);
2049  MatNx3 SubMat2(NModes, 0.);
2050 
2051  integer iOffset1 = iRigidOffset + 2*NModes + 12*iStrNodem1;
2052  integer iOffset2 = iRigidOffset + iGetInitialNumDof() + 12*iStrNodem1;
2053  if (pModalNode) {
2054  for (unsigned int iCnt = 1; iCnt <= 3; iCnt++) {
2055  /* Contributo di forza all'equazione
2056  * della forza, nodo 1 */
2057  WM.IncCoef(iCnt, iOffset1 + iCnt, 1.);
2058 
2059  /* Contrib. di der. di forza all'eq. della der.
2060  * della forza, nodo 1 */
2061  WM.IncCoef(6 + iCnt, iOffset1 + 6 + iCnt, 1.);
2062 
2063  /* Equazione di vincolo, nodo 1 */
2064  WM.DecCoef(iOffset1 + iCnt, iCnt, 1.);
2065 
2066  /* Derivata dell'equazione di vincolo, nodo 1 */
2067  WM.DecCoef(iOffset1 + 6 + iCnt, 6 + iCnt, 1.);
2068  }
2069  }
2070 
2071  for (unsigned int iCnt = 1; iCnt <= 3; iCnt++) {
2072  /* Equazione di vincolo, nodo 2 */
2073  WM.IncCoef(iOffset1 + iCnt, iOffset2 + iCnt, 1.);
2074 
2075  /* Derivata dell'equazione di vincolo, nodo 2 */
2076  WM.IncCoef(iOffset1 + 6 + iCnt, iOffset2 + 6 + iCnt, 1.);
2077 
2078  /* Contributo di forza all'equazione della forza,
2079  * nodo 2 */
2080  WM.DecCoef(iOffset2 + iCnt, iOffset1 + iCnt, 1.);
2081 
2082  /* Contrib. di der. di forza all'eq. della der.
2083  * della forza, nodo 2 */
2084  WM.DecCoef(iOffset2 + 6 + iCnt, iOffset1 + 6 + iCnt, 1.);
2085  }
2086 
2087  /* Distanza nel sistema globale */
2088  const Vec3& d1Tmp(d1tot);
2089  Vec3 d2Tmp(R2*SND[iStrNodem1].OffsetMB);
2090 
2091  /* Matrici F/\d1/\, -F/\d2/\ , F/\omega1/\ */
2092  Mat3x3 FWedged1Wedge(MatCrossCross, F, d1Tmp);
2093  Mat3x3 FWedged2Wedge(MatCrossCross, F, d2Tmp);
2094  Mat3x3 FWedgeO1Wedge(MatCrossCross, F, Omega1);
2095 
2096  /* Matrici (omega1/\d1)/\, -(omega2/\d2)/\ */
2097  Mat3x3 O1Wedged1Wedge(MatCross, Omega1.Cross(d1Tmp));
2098  Mat3x3 O2Wedged2Wedge(MatCross, d2Tmp.Cross(Omega2));
2099 
2100  /* d1Prime= w1/\d1 + R*PHIt*b */
2101  Mat3xN R1PHIt(NModes);
2102  R1PHIt.LeftMult(R, PHIt);
2103  Vec3 d1Prime(Omega1.Cross(d1Tmp) + R1PHIt*b);
2104 
2105  Mat3x3 FWedge(MatCross, F);
2106  if (pModalNode) {
2107  /* Equazione di momento, nodo 1 */
2108  WM.Add(3 + 1, 3 + 1, FWedged1Wedge);
2109  WM.Add(3 + 1, iOffset1 + 1, Mat3x3(MatCross, d1Tmp));
2110 
2111  /* Equazione di momento, contributo modale */
2112  SubMat1.LeftMult(FWedge*R, PHIt);
2113  WM.Sub(3 + 1, iRigidOffset + 1, SubMat1);
2114 
2115  /* Eq. di equilibrio ai modi */
2116  SubMat2.RightMult(PHItT, RT*FWedge);
2117  WM.Add(iRigidOffset + 1, 3 + 1, SubMat2);
2118  }
2119 
2120  /* Equazione di momento, nodo 2 */
2121  WM.Sub(iOffset2 + 3 + 1,iOffset2 + 3 + 1, FWedged2Wedge);
2122  WM.Sub(iOffset2 + 3 + 1,iOffset1 + 1, Mat3x3(MatCross, d2Tmp));
2123 
2124  /* Eq. di equilibrio ai modi */
2125  SubMat2.RightMult(PHItT, RT);
2126  WM.Add(iRigidOffset + 1, iRigidOffset + 2*NModes + 1, SubMat2);
2127 
2128  if (pModalNode) {
2129  /* derivata dell'equazione di momento, nodo 1 */
2130  WM.Add(9 + 1, 3 + 1,
2131  Mat3x3(MatCrossCross, FPrime, d1Tmp) + F.Cross(Mat3x3(MatCrossCross, Omega1, d1Tmp))
2132  + Mat3x3(MatCrossCross, F, R*(PHIt*b)));
2133  WM.Add(9 + 1, 9 + 1, FWedged1Wedge);
2134  WM.Add(9 + 1, iOffset1 + 1, O1Wedged1Wedge + Mat3x3(MatCross, R*(PHIt*b)));
2135  WM.Add(9 + 1, iOffset1 + 6 + 1, Mat3x3(MatCross, d1Tmp));
2136 
2137  /* derivata dell'equazione di momento, contributo modale */
2138  SubMat1.LeftMult((-FWedgeO1Wedge - Mat3x3(MatCross, FPrime))*R, PHIt);
2139  WM.Add(9 + 1, iRigidOffset + 1, SubMat1);
2140  SubMat1.LeftMult(-FWedge*R, PHIt);
2141  WM.Add(9 + 1, iRigidOffset + NModes + 1, SubMat1);
2142 
2143  /* derivata dell'eq. di equilibrio ai modi */
2144  SubMat2.RightMult(PHItT, RT*FWedge);
2145  WM.Add(iRigidOffset + NModes + 1, 9 + 1, SubMat2);
2146  SubMat2.RightMult(PHItT, RT*FWedgeO1Wedge);
2147  WM.Add(iRigidOffset + NModes + 1, 3 + 1, SubMat2);
2148  }
2149 
2150  SubMat2.RightMult(PHItT, RT);
2151  WM.Add(iRigidOffset + NModes + 1, iRigidOffset + 2*NModes + 6 + 1, SubMat2);
2152 
2153  /* Derivata dell'equazione di momento, nodo 2 */
2154  WM.Sub(iOffset2 + 9 + 1, iOffset2 + 3 + 1,
2155  Mat3x3(MatCrossCross, FPrime, d2Tmp)
2156  + F.Cross(Mat3x3(MatCrossCross, Omega2, d2Tmp)));
2157  WM.Sub(iOffset2 + 9 + 1, iOffset2 + 9 + 1, FWedged2Wedge);
2158  WM.Add(iOffset2 + 9 + 1, iOffset1 + 1, O2Wedged2Wedge);
2159  WM.Sub(iOffset2 + 9 + 1, iOffset1 + 6 + 1, Mat3x3(MatCross, d2Tmp));
2160 
2161  /* Equazione di vincolo */
2162  if (pModalNode) {
2163  WM.Add(iOffset1 + 1, 3 + 1, Mat3x3(MatCross, d1Tmp));
2164  }
2165  WM.Sub(iOffset1 + 1, iOffset2 + 3 + 1, Mat3x3(MatCross, d2Tmp));
2166 
2167  /* Equazione di vincolo, contributo modale */
2168  SubMat1.LeftMult(R, PHIt);
2169  WM.Sub(iOffset1 + 1, iRigidOffset + 1, SubMat1);
2170 
2171  /* Derivata dell'equazione di vincolo */
2172  if (pModalNode) {
2173  WM.Add(iOffset1 + 6 + 1, 3 + 1, O1Wedged1Wedge + Mat3x3(MatCross, R*(PHIt*b)));
2174  WM.Add(iOffset1 + 6 + 1, 9 + 1, Mat3x3(MatCross, d1Tmp));
2175  }
2176  WM.Add(iOffset1 + 6 + 1, iOffset2 + 3 + 1, O2Wedged2Wedge);
2177  WM.Sub(iOffset1 + 6 + 1, iOffset2 + 9 + 1, Mat3x3(MatCross, d2Tmp));
2178 
2179  /* Derivata dell'equazione di vincolo, contributo modale */
2180  SubMat1.LeftMult(Omega1.Cross(R), PHIt);
2181  WM.Sub(iOffset1 + 6 + 1, iRigidOffset + 1, SubMat1);
2182  SubMat1.LeftMult(R, PHIt);
2183  WM.Sub(iOffset1 + 6 + 1, iRigidOffset + NModes + 1, SubMat1);
2184 
2185  /* giunto prismatico */
2186  Vec3 M(XCurr, iFlexIndex + 2*NModes + 12*iStrNodem1 + 3 + 1);
2187  Vec3 MPrime(XCurr, iFlexIndex + 2*NModes + 12*iStrNodem1 + 9 + 1);
2188  Vec3 e1tota(R1tot.GetVec(1));
2189  Vec3 e2tota(R1tot.GetVec(2));
2190  Vec3 e3tota(R1tot.GetVec(3));
2191  Vec3 e1b(R2.GetVec(1));
2192  Vec3 e2b(R2.GetVec(2));
2193  Vec3 e3b(R2.GetVec(3));
2194 
2195  /* */
2196  Mat3x3 MWedge(Mat3x3(MatCrossCross, e3b, e2tota*M(1))
2197  + Mat3x3(MatCrossCross, e1b, e3tota*M(2))
2198  + Mat3x3(MatCrossCross, e2b, e1tota*M(3)));
2199 
2200  /* Equilibrio */
2201  if (pModalNode) {
2202  WM.Add(3 + 1, 3 + 1, MWedge);
2203  WM.SubT(3 + 1, iOffset2 + 3 + 1, MWedge);
2204 
2205  WM.AddT(iOffset2 + 3 + 1, 3 + 1, MWedge);
2206  }
2207  WM.Sub(iOffset2 + 3 + 1, iOffset2 + 3 + 1, MWedge);
2208 
2209  /* Equilibrio dei momenti, termini aggiuntivi modali */
2210  Vec3 e1ra(R.GetVec(1));
2211  Vec3 e2ra(R.GetVec(2));
2212  Vec3 e3ra(R.GetVec(3));
2213  Mat3x3 M1Wedge(Mat3x3(MatCrossCross, e3b, e2ra*M(1))
2214  + Mat3x3(MatCrossCross, e1b, e3ra*M(2))
2215  + Mat3x3(MatCrossCross, e2b, e1ra*M(3)));
2216 
2217  SubMat1.LeftMult(M1Wedge, PHIr);
2218  if (pModalNode) {
2219  WM.Add(3 + 1, iRigidOffset + 1, SubMat1);
2220  }
2221  WM.Sub(iOffset2 + 3 + 1, iRigidOffset + 1, SubMat1);
2222 
2223  /* Equilibrio ai modi */
2224  if (pModalNode) {
2225  SubMat2.RightMult(PHIrT, R1tot.MulTM(MWedge));
2226  WM.Add(iRigidOffset + 1, 3 + 1, SubMat2);
2227  }
2228  SubMat2.RightMult(PHIrT, R1tot.MulTMT(MWedge));
2229  WM.Sub(iRigidOffset + 1, iOffset2 + 3 + 1, SubMat2);
2230 
2231  Vec3 MA(Mat3x3(e2tota.Cross(e3b), e3tota.Cross(e1b),
2232  e1tota.Cross(e2b))*M);
2233  if (pModalNode) {
2234  SubMat2.RightMult(PHIrT, R1tot.MulTM(Mat3x3(MatCross, MA)));
2235  WM.Add(iRigidOffset + 1, 3 + 1, SubMat2);
2236  }
2237 
2238  Mat3x3 R1TMAWedge(MatCross, RT*MA);
2239  SubMat1.LeftMult(R1TMAWedge, PHIr);
2240  Mat3xN SubMat3(NModes);
2241  MatNxN SubMat4(NModes);
2242  SubMat3.LeftMult(R1tot.MulTM(M1Wedge), PHIr);
2243  SubMat1 += SubMat3;
2244  SubMat4.Mult(PHIrT, SubMat1);
2245  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
2246  for (unsigned int kMode = 1; kMode <= NModes; kMode++) {
2247  WM.IncCoef(iRigidOffset + jMode, iRigidOffset + kMode,
2248  SubMat4(jMode, kMode));
2249  }
2250  }
2251 
2252  /* Derivate dell'equilibrio */
2253  if (pModalNode) {
2254  WM.Add(9 + 1, 9 + 1, MWedge);
2255  WM.SubT(9 + 1, iOffset2 + 9 + 1, MWedge);
2256 
2257  WM.AddT(iOffset2 + 9 + 1, 9 + 1, MWedge);
2258  }
2259  WM.Sub(iOffset2 + 9 + 1, iOffset2 + 9 + 1, MWedge);
2260 
2261  MWedge = ((Mat3x3(MatCrossCross, e3b, Omega1) + Mat3x3(MatCross, Omega2.Cross(e3b*M(1))))
2262  + Mat3x3(MatCross, e3b*MPrime(1)))*Mat3x3(MatCross, e2tota)
2263  + ((Mat3x3(MatCrossCross, e1b, Omega1) + Mat3x3(MatCross, Omega2.Cross(e1b*M(2))))
2264  + Mat3x3(MatCross, e1b*MPrime(2)))*Mat3x3(MatCross, e3tota)
2265  + ((Mat3x3(MatCrossCross, e2b, Omega1) + Mat3x3(MatCross, Omega2.Cross(e2b*M(3))))
2266  + Mat3x3(MatCross, e2b*MPrime(3)))*Mat3x3(MatCross, e1tota);
2267 
2268  if (pModalNode) {
2269  WM.Add(9 + 1, 3 + 1, MWedge);
2270  WM.Sub(iOffset2 + 9 + 1, 3 + 1, MWedge);
2271  }
2272 
2273  MWedge = ((Mat3x3(MatCrossCross, e2tota, Omega2) + Mat3x3(MatCross, Omega1.Cross(e2tota*M(1))))
2274  + Mat3x3(MatCross, e2tota*MPrime(1)))*Mat3x3(MatCross, e3b)
2275  + ((Mat3x3(MatCrossCross, e3tota, Omega2) + Mat3x3(MatCross, Omega1.Cross(e3tota*M(2))))
2276  + Mat3x3(MatCross, e3tota*MPrime(2)))*Mat3x3(MatCross, e1b)
2277  + ((Mat3x3(MatCrossCross, e1tota, Omega2) + Mat3x3(MatCross, Omega1.Cross(e1tota*M(3))))
2278  + Mat3x3(MatCross, e1tota*MPrime(3)))*Mat3x3(MatCross, e2b);
2279 
2280  if (pModalNode) {
2281  WM.Sub(9 + 1, iOffset2 + 3 + 1, MWedge);
2282  }
2283  WM.Add(iOffset2 + 9 + 1, iOffset2 + 3 + 1, MWedge);
2284 
2285  /* Derivate dell'equilibrio, termini aggiuntivi modali */
2286  SubMat1.LeftMult(M1Wedge, PHIr);
2287  if (pModalNode) {
2288  WM.Add(9 + 1, iRigidOffset + NModes + 1, SubMat1);
2289  }
2290  WM.Sub(iOffset2 + 9 + 1, iRigidOffset + NModes + 1, SubMat1);
2291 
2292  Vec3 v1(e2tota.Cross(e3b));
2293  Vec3 v2(e3tota.Cross(e1b));
2294  Vec3 v3(e1tota.Cross(e2b));
2295 
2296  /* Error handling: il programma si ferma,
2297  * pero' segnala dov'e' l'errore */
2298  if (v1.Dot() < std::numeric_limits<doublereal>::epsilon()
2299  || v2.Dot() < std::numeric_limits<doublereal>::epsilon()
2300  || v3.Dot() < std::numeric_limits<doublereal>::epsilon())
2301  {
2302  silent_cerr("Modal(" << GetLabel() << "):"
2303  << "warning, first node hinge axis "
2304  "and second node hinge axis "
2305  "are (nearly) orthogonal; aborting..."
2306  << std::endl);
2307  throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
2308  }
2309 
2310  MWedge = Mat3x3(v1, v2, v3);
2311 
2312  /* Equilibrio */
2313  if (pModalNode) {
2314  WM.Add(3 + 1, iOffset1 + 3 + 1, MWedge);
2315  }
2316  WM.Sub(iOffset2 + 3 + 1, iOffset1 + 3 + 1, MWedge);
2317 
2318  SubMat2.RightMult(PHIrT, R1tot.MulTM(MWedge));
2319  WM.Add(iRigidOffset + 1, iOffset1 + 3 + 1, SubMat2);
2320 
2321  /* Derivate dell'equilibrio */
2322  if (pModalNode) {
2323  WM.Add(9 + 1, iOffset1 + 9 + 1, MWedge);
2324  }
2325  WM.Sub(iOffset2 + 9 + 1, iOffset1 + 9 + 1, MWedge);
2326 
2327  // NOTE: from this point on, MWedge = MWedge.Transpose();
2328 
2329  /* Equaz. di vincolo */
2330  if (pModalNode) {
2331  WM.AddT(iOffset1 + 3 + 1, 3 + 1, MWedge);
2332  }
2333  WM.SubT(iOffset1 + 3 + 1, iOffset2 + 3 + 1, MWedge);
2334 
2335  /* Equaz. di vincolo: termine aggiuntivo
2336  * dovuto alla flessibilita' */
2337  Vec3 u1(e2ra.Cross(e3b));
2338  Vec3 u2(e3ra.Cross(e1b));
2339  Vec3 u3(e1ra.Cross(e2b));
2340 
2341  M1Wedge = (Mat3x3(u1, u2, u3)).Transpose();
2342  SubMat1.LeftMult(M1Wedge, PHIr);
2343  WM.Add(iOffset1 + 3 + 1, iRigidOffset + 1, SubMat1);
2344 
2345  /* Derivate delle equaz. di vincolo */
2346  if (pModalNode) {
2347  WM.AddT(iOffset1 + 9 + 1, 9 + 1, MWedge);
2348  }
2349  WM.SubT(iOffset1 + 9 + 1, iOffset2 + 9 + 1, MWedge);
2350 
2351  v1 = e3b.Cross(e2tota.Cross(Omega1))
2352  + e2tota.Cross(Omega2.Cross(e3b));
2353  v2 = e1b.Cross(e3tota.Cross(Omega1))
2354  + e3tota.Cross(Omega2.Cross(e1b));
2355  v3 = e2b.Cross(e1tota.Cross(Omega1))
2356  + e1tota.Cross(Omega2.Cross(e2b));
2357 
2358  MWedge = Mat3x3(v1, v2, v3);
2359 
2360  /* Derivate dell'equilibrio */
2361  if (pModalNode) {
2362  WM.Add(9 + 1, iOffset1 + 3 + 1, MWedge);
2363  }
2364  WM.Sub(iOffset2 + 9 + 1, iOffset1 + 3 + 1, MWedge);
2365 
2366  /* Derivate delle equaz. di vincolo */
2367  Omega1 = Omega1 - Omega2;
2368 
2369  v1 = e2tota.Cross(e3b.Cross(Omega1));
2370  Vec3 v1p(e3b.Cross(Omega1.Cross(e2tota)));
2371  v2 = e3tota.Cross(e1b.Cross(Omega1));
2372  Vec3 v2p(e1b.Cross(Omega1.Cross(e3tota)));
2373  v3 = e1tota.Cross(e2b.Cross(Omega1));
2374  Vec3 v3p(e2b.Cross(Omega1.Cross(e1tota)));
2375 
2376  if (pModalNode) {
2377  for (unsigned int iCnt = 1; iCnt <= 3; iCnt++) {
2378  doublereal d;
2379 
2380  d = v1(iCnt);
2381  WM.IncCoef(iOffset1 + 9 + 1, 3 + iCnt, d);
2382 
2383  d = v2(iCnt);
2384  WM.IncCoef(iOffset1 + 9 + 2, 3 + iCnt, d);
2385 
2386  d = v3(iCnt);
2387  WM.IncCoef(iOffset1 + 9 + 3, 3 + iCnt, d);
2388  }
2389  }
2390 
2391  for (unsigned int iCnt = 1; iCnt <= 3; iCnt++) {
2392  doublereal d;
2393 
2394  d = v1p(iCnt);
2395  WM.IncCoef(iOffset1 + 9 + 1, iOffset2 + 3 + iCnt, d);
2396 
2397  d = v2p(iCnt);
2398  WM.IncCoef(iOffset1 + 9 + 2, iOffset2 + 3 + iCnt, d);
2399 
2400  d = v3p(iCnt);
2401  WM.IncCoef(iOffset1 + 9 + 3, iOffset2 + 3 + iCnt, d);
2402  }
2403 
2404  } /* fine ciclo sui nodi vincolati */
2405 
2406  return WorkMat;
2407 }
2408 
2409 /* Contributo al residuo durante l'assemblaggio iniziale */
2410 SubVectorHandler&
2411 Modal::InitialAssRes(SubVectorHandler& WorkVec,
2412  const VectorHandler& XCurr)
2413 {
2414  DEBUGCOUT("Entering Modal::InitialAssRes()" << std::endl);
2415 
2416  integer iNumRows;
2417  integer iNumCols;
2418  InitialWorkSpaceDim(&iNumRows, &iNumCols);
2419  WorkVec.ResizeReset(iNumRows);
2420 
2421  VecN Ka(NModes);
2422 
2423  if (pModalNode) {
2424  integer iRigidIndex = pModalNode->iGetFirstIndex();
2425 
2426  for (unsigned int iCnt = 1; iCnt <= iRigidOffset; iCnt++) {
2427  WorkVec.PutRowIndex(iCnt, iRigidIndex + iCnt);
2428  }
2429  }
2430 
2431  integer iFlexIndex = iGetFirstIndex();
2432 
2433  for (unsigned int iCnt = 1; iCnt <= iGetInitialNumDof(); iCnt++) {
2434  WorkVec.PutRowIndex(iRigidOffset + iCnt, iFlexIndex + iCnt);
2435  }
2436 
2437  for (unsigned iStrNodem1 = 0; iStrNodem1 < NStrNodes; iStrNodem1++) {
2438  integer iNodeFirstPosIndex =
2439  SND[iStrNodem1].pNode->iGetFirstPositionIndex();
2440  integer iNodeFirstVelIndex = iNodeFirstPosIndex + 6;
2441 
2442  integer iOffset2 = iRigidOffset + iGetInitialNumDof() + 12*iStrNodem1;
2443  for (unsigned int iCnt = 1; iCnt <= 6; iCnt++) {
2444  WorkVec.PutRowIndex(iOffset2 + iCnt,
2445  iNodeFirstPosIndex + iCnt);
2446  WorkVec.PutRowIndex(iOffset2 + 6 + iCnt,
2447  iNodeFirstVelIndex + iCnt);
2448  }
2449  }
2450 
2451  /* updates modal displacements and velocities: a, b (== aP) */
2452  for (unsigned int iCnt = 1; iCnt <= NModes; iCnt++) {
2453  a.Put(iCnt, XCurr(iFlexIndex + iCnt));
2454  b.Put(iCnt, XCurr(iFlexIndex + NModes + iCnt));
2455  }
2456 
2457  /* updates modal forces: K*a + C*aP */
2458  for (unsigned int iCnt = 1; iCnt <= NModes; iCnt++) {
2459  doublereal temp = 0.;
2460 
2461  for (unsigned int jCnt = 1; jCnt <= NModes; jCnt++) {
2462  temp += pModalStiff->dGet(iCnt, jCnt)*a(jCnt)
2463  + pModalDamp->dGet(iCnt, jCnt)*b(jCnt);
2464  }
2465  WorkVec.DecCoef(iRigidOffset + iCnt, temp);
2466  }
2467 
2468  /* equazioni di vincolo */
2469 
2470  /* Recupera i dati */
2471  Vec3 v1;
2472  Vec3 Omega1;
2473 
2474  if (pModalNode) {
2475  v1 = pModalNode->GetVCurr();
2476  R = pModalNode->GetRCurr();
2477  RT = R.Transpose();
2478  Omega1 = pModalNode->GetWCurr();
2479 
2480  } else {
2481  v1 = ::Zero3;
2482  Omega1 = ::Zero3;
2483  }
2484 
2485  for (unsigned int iStrNode = 1; iStrNode <= NStrNodes; iStrNode++) {
2486  unsigned int iStrNodem1 = iStrNode - 1;
2487  Mat3xN PHIt(NModes,0), PHIr(NModes,0);
2488 
2489  integer iOffset1 = iRigidOffset + 2*NModes + 12*iStrNodem1;
2490  integer iOffset2 = iRigidOffset + iGetInitialNumDof() + 12*iStrNodem1;
2491 
2492  for (unsigned int iCnt = 1; iCnt <= 3; iCnt++) {
2493  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
2494  PHIt.Put(iCnt, jMode, pPHIt->dGet(iCnt, (jMode - 1)*NStrNodes + iStrNode));
2495  PHIr.Put(iCnt, jMode, pPHIr->dGet(iCnt, (jMode - 1)*NStrNodes + iStrNode));
2496  }
2497  }
2498 
2499  MatNx3 PHItT(NModes), PHIrT(NModes);
2500  PHItT.Transpose(PHIt);
2501  PHIrT.Transpose(PHIr);
2502 
2503  Vec3 d1tot = R*(SND[iStrNodem1].OffsetFEM + PHIt*a);
2504  Mat3x3 R1tot = R*Mat3x3(1., PHIr*a);
2505 
2506  const Vec3& x2(SND[iStrNodem1].pNode->GetXCurr());
2507  const Vec3& v2(SND[iStrNodem1].pNode->GetVCurr());
2508  const Mat3x3& R2(SND[iStrNodem1].pNode->GetRCurr());
2509  const Vec3& Omega2(SND[iStrNodem1].pNode->GetWCurr());
2510  Vec3 F(XCurr, iFlexIndex + 2*NModes + 12*iStrNodem1 + 1);
2511  Vec3 FPrime(XCurr, iFlexIndex + 2*NModes + 12*iStrNodem1 + 6 + 1);
2512 
2513  /* cerniera sferica */
2514 
2515  /* Distanza nel sistema globale */
2516  const Vec3& d1Tmp(d1tot);
2517  Vec3 d2Tmp(R2*SND[iStrNodem1].OffsetMB);
2518 
2519  /* Vettori omega1/\d1, -omega2/\d2 */
2520  Vec3 O1Wedged1(Omega1.Cross(d1Tmp));
2521  Vec3 O2Wedged2(Omega2.Cross(d2Tmp));
2522 
2523  /* d1Prime= w1/\d1 + R*PHIt*b */
2524  Mat3xN R1PHIt(NModes);
2525  R1PHIt.LeftMult(R, PHIt);
2526  Vec3 d1Prime(O1Wedged1 + R1PHIt*b);
2527 
2528  /* Equazioni di equilibrio, nodo 1 */
2529  if (pModalNode) {
2530  WorkVec.Sub(1, F);
2531  WorkVec.Add(3 + 1, F.Cross(d1Tmp)); /* F/\d = -d/\F */
2532  }
2533 
2534  /* Eq. d'equilibrio ai modi */
2535  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
2536  doublereal temp = 0.;
2537  for (unsigned int iCnt = 1; iCnt <= 3; iCnt++) {
2538  temp += PHIt(iCnt, jMode)*(RT*F).dGet(iCnt);
2539  }
2540  WorkVec.DecCoef(iRigidOffset + jMode, temp);
2541  }
2542 
2543  /* Derivate delle equazioni di equilibrio, nodo 1 */
2544  if (pModalNode) {
2545  WorkVec.Sub(6 + 1, FPrime);
2546  WorkVec.Add(9 + 1, FPrime.Cross(d1Tmp) - d1Prime.Cross(F));
2547  }
2548 
2549  /* Derivata dell'eq. di equilibrio ai modi */
2550  MatNx3 PHItTR1T(NModes);
2551  PHItTR1T.RightMult(PHItT, RT);
2552  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
2553  doublereal temp = 0.;
2554 
2555  for (unsigned int iCnt = 1; iCnt <= 3; iCnt++) {
2556  temp += PHItTR1T(jMode, iCnt)*(Omega1.Cross(F) - FPrime).dGet(iCnt);
2557  }
2558  WorkVec.IncCoef(iRigidOffset + NModes + jMode, temp);
2559  }
2560 
2561  /* Equazioni di equilibrio, nodo 2 */
2562  WorkVec.Add(iOffset2 + 1, F);
2563  WorkVec.Add(iOffset2 + 3 + 1, d2Tmp.Cross(F));
2564 
2565  /* Derivate delle equazioni di equilibrio, nodo 2 */
2566  WorkVec.Add(iOffset2 + 6 + 1, FPrime);
2567  WorkVec.Add(iOffset2 + 9 + 1, d2Tmp.Cross(FPrime) + O2Wedged2.Cross(F));
2568 
2569  /* Equazione di vincolo */
2570  WorkVec.Add(iOffset1 + 1, x + d1Tmp - x2 - d2Tmp);
2571 
2572  /* Derivata dell'equazione di vincolo */
2573  WorkVec.Add(iOffset1 + 6 + 1, v1 + d1Prime - v2 - O2Wedged2);
2574 
2575  /* giunto prismatico */
2576  Vec3 M(XCurr, iFlexIndex + 2*NModes + 12*iStrNodem1 + 3 + 1);
2577  Vec3 MPrime(XCurr, iFlexIndex + 2*NModes + 12*iStrNodem1 + 9 + 1);
2578 
2579  Vec3 e1a(R1tot.GetVec(1));
2580  Vec3 e2a(R1tot.GetVec(2));
2581  Vec3 e3a(R1tot.GetVec(3));
2582  Vec3 e1b(R2.GetVec(1));
2583  Vec3 e2b(R2.GetVec(2));
2584  Vec3 e3b(R2.GetVec(3));
2585 
2586  Vec3 MTmp(e2a.Cross(e3b*M(1))
2587  + e3a.Cross(e1b*M(2))
2588  + e1a.Cross(e2b*M(3)));
2589 
2590  /* Equazioni di equilibrio, nodo 1 */
2591  if (pModalNode) {
2592  WorkVec.Sub(3 + 1, MTmp);
2593  }
2594 
2595  /* Equazioni di equilibrio, nodo 2 */
2596  WorkVec.Add(iOffset2 + 3 + 1, MTmp);
2597 
2598  /* Equazioni di equilibrio, contributo modale */
2599  Vec3 MTmpRel(R1tot.MulTV(MTmp));
2600  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
2601  WorkVec.DecCoef(iRigidOffset + jMode, PHIrT.GetVec(jMode)*MTmpRel);
2602  }
2603 
2604  /* eaPrime = w/\ea + R*[(PHIr*b)/\]ia */
2605  Mat3x3 Tmp = R*Mat3x3(MatCross, PHIr*b);
2606  Vec3 e1aPrime = Omega1.Cross(e1a) + Tmp.GetVec(1);
2607  Vec3 e2aPrime = Omega1.Cross(e2a) + Tmp.GetVec(2);
2608  Vec3 e3aPrime = Omega1.Cross(e3a) + Tmp.GetVec(3);
2609  Vec3 MTmpPrime(::Zero3);
2610  // FIXME: check (always M(1)?)
2611  MTmpPrime =
2612  (e2a.Cross(Omega2.Cross(e3b)) - e3b.Cross(e2aPrime))*M(1)
2613  + (e3a.Cross(Omega2.Cross(e1b)) - e1b.Cross(e3aPrime))*M(2)
2614  + (e1a.Cross(Omega2.Cross(e2b)) - e2b.Cross(e1aPrime))*M(3)
2615  + e2a.Cross(e3b*MPrime(1))
2616  + e3a.Cross(e1b*MPrime(2))
2617  + e1a.Cross(e2b*MPrime(3));
2618 
2619  /* Derivate delle equazioni di equilibrio, nodo 1 */
2620  if (pModalNode) {
2621  WorkVec.Sub(9 + 1, MTmpPrime);
2622  }
2623 
2624  /* Derivate delle equazioni di equilibrio, nodo 2 */
2625  WorkVec.Add(iOffset2 + 9 + 1, MTmpPrime);
2626 
2627  /* Derivate delle equazioni di equilibrio, contributo modale */
2628  MatNx3 SubMat1(NModes);
2629  SubMat1.RightMult(PHIrT, R1tot.Transpose());
2630 
2631  /* FIXME: temporary ((PHIr*b).Cross(RT*MTmp)) */
2632  Vec3 T1 = MTmpPrime - Omega1.Cross(MTmp);
2633  Vec3 T2 = (PHIr*b).Cross(RT*MTmp);
2634 
2635  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
2636  doublereal temp = 0;
2637 
2638  for (unsigned int iCnt = 1; iCnt <= 3; iCnt++) {
2639  temp += SubMat1(jMode, iCnt)*T1(iCnt)
2640  - PHIrT(jMode, iCnt)*T2(iCnt);
2641  }
2642  WorkVec.DecCoef(iRigidOffset + NModes + jMode, temp);
2643  }
2644 
2645  /* Equazioni di vincolo di rotazione */
2646  WorkVec.DecCoef(iOffset1 + 3 + 1, e3b.Dot(e2a));
2647  WorkVec.DecCoef(iOffset1 + 3 + 2, e1b.Dot(e3a));
2648  WorkVec.DecCoef(iOffset1 + 3 + 3, e2b.Dot(e1a));
2649 
2650  /* Derivate delle equazioni di vincolo di rotazione */
2651  WorkVec.IncCoef(iOffset1 + 9 + 1,
2652  e3b.Dot(Omega2.Cross(e2a) - e2aPrime));
2653  WorkVec.IncCoef(iOffset1 + 9 + 2,
2654  e1b.Dot(Omega2.Cross(e3a) - e3aPrime));
2655  WorkVec.IncCoef(iOffset1 + 9 + 3,
2656  e2b.Dot(Omega2.Cross(e1a) - e1aPrime));
2657 
2658  } /* fine equazioni di vincolo */
2659 
2660  return WorkVec;
2661 }
2662 
2663 void
2664 Modal::SetInitialValue(VectorHandler& X)
2665 {
2666  /* inizializza la soluzione prima dell'assemblaggio iniziale */
2667 
2668  int iFlexIndex = iGetFirstIndex();
2669 
2670  for (unsigned int iCnt = 1; iCnt <= NModes; iCnt++) {
2671  /* modal multipliers */
2672  X.PutCoef(iFlexIndex + iCnt, a(iCnt));
2673 
2674  /* derivatives of modal multipliers */
2675  X.PutCoef(iFlexIndex + NModes + iCnt, b(iCnt));
2676  }
2677 
2678  if (pModalNode) {
2679  integer iRigidIndex = pModalNode->iGetFirstIndex();
2680 
2681  X.Put(iRigidIndex + 6 + 1, pModalNode->GetVCurr());
2682  X.Put(iRigidIndex + 9 + 1, pModalNode->GetWCurr());
2683  }
2684 }
2685 
2686 void
2687 Modal::SetValue(DataManager *pDM,
2688  VectorHandler& X, VectorHandler& XP,
2689  SimulationEntity::Hints *ph)
2690 {
2691  /* inizializza la soluzione e la sua derivata
2692  * subito dopo l'assemblaggio iniziale
2693  * e prima del calcolo delle derivate */
2694 
2695  int iFlexIndex = iGetFirstIndex();
2696 
2697  for (unsigned int iCnt = 1; iCnt <= NModes; iCnt++) {
2698  /* modal multipliers */
2699  X.PutCoef(iFlexIndex + iCnt, a(iCnt));
2700 
2701  /* derivatives of modal multipliers */
2702  X.PutCoef(iFlexIndex + NModes + iCnt, b(iCnt));
2703  XP.PutCoef(iFlexIndex + iCnt, b(iCnt));
2704  }
2705 
2706  if (pModalNode) {
2707  integer iRigidIndex = pModalNode->iGetFirstIndex();
2708 
2709  X.Put(iRigidIndex + 6 + 1, pModalNode->GetVCurr());
2710  X.Put(iRigidIndex + 9 + 1, pModalNode->GetWCurr());
2711  XP.Put(iRigidIndex + 6 + 1, pModalNode->GetXPPCurr());
2712  XP.Put(iRigidIndex + 9 + 1, pModalNode->GetWPCurr());
2713  }
2714 }
2715 
2716 #if 0
2717 /* Aggiorna dati durante l'iterazione fittizia iniziale */
2718 void
2719 Modal::DerivativesUpdate(const VectorHandler& X,
2720  const VectorHandler& XP)
2721 {
2722  // NOTE: identical to SetValue()...
2723  int iFlexIndex = iGetFirstIndex();
2724 
2725  for (unsigned int iCnt = 1; iCnt <= NModes; iCnt++) {
2726  /* modal multipliers */
2727  const_cast<VectorHandler &>(X).PutCoef(iFlexIndex + iCnt, a.dGet(iCnt));
2728 
2729  /* derivatives of modal multipliers */
2730  const_cast<VectorHandler &>(X).PutCoef(iFlexIndex + NModes + iCnt, b.dGet(iCnt));
2731  const_cast<VectorHandler &>(XP).PutCoef(iFlexIndex + iCnt, b.dGet(iCnt));
2732  }
2733 }
2734 #endif
2735 
2736 unsigned int
2737 Modal::iGetNumPrivData(void) const
2738 {
2739  return 3*NModes + 18*NFEMNodes;
2740 }
2741 
2742 unsigned int
2743 Modal::iGetPrivDataIdx(const char *s) const
2744 {
2745  /*
2746  * q[n] | qP[n] | qPP[n]
2747  *
2748  * where n is the 1-based mode number
2749  *
2750  * q[#i] | qP[#i] | qPP[#i]
2751  *
2752  * where i is the 1-based mode index (if only a subset of modes is used)
2753  *
2754  * x[n,i] | xP[n,i] | xPP[n,i]
2755  * w[n,i] | wP[n,i]
2756  *
2757  * where n is the FEM node label and i is the index number (1-3)
2758  */
2759 
2760  unsigned p = 0;
2761  char what = s[0];
2762 
2763  /* che cosa e' richiesto? */
2764  switch (what) {
2765  case 'q':
2766  /* modal dofs */
2767  break;
2768 
2769  case 'x':
2770  /* structural nodes positions */
2771  break;
2772 
2773  case 'w':
2774  /* structural nodes angular velocities */
2775  p = 1;
2776  break;
2777 
2778  default:
2779  /* unknown priv data */
2780  return 0;
2781  }
2782 
2783  while ((++s)[0] == 'P') {
2784  p++;
2785  if (p > 2) {
2786  /* no more than two derivative levels allowed */
2787  return 0;
2788  }
2789  }
2790 
2791  if (s[0] != '[') {
2792  return 0;
2793  }
2794  s++;
2795 
2796  /* trova la parentesi chiusa (e il terminatore di stringa) */
2797  const char *end = std::strchr(s, ']');
2798  if (end == 0 || end[1] != '\0') {
2799  return 0;
2800  }
2801 
2802  if (what == 'q') {
2803  bool bIdx(false);
2804 
2805  if (s[0] == '#') {
2806  bIdx = true;
2807  s++;
2808  }
2809 
2810  /* buffer per numero (dimensione massima: 32 bit) */
2811  char buf[sizeof("18446744073709551615") + 1];
2812  size_t len = end - s;
2813 
2814  ASSERT(len < sizeof(buf));
2815 
2816  strncpy(buf, s, len);
2817  buf[len] = '\0';
2818 
2819  /* leggi il numero */
2820  if (buf[0] == '-') {
2821  return 0;
2822  }
2823 
2824  errno = 0;
2825  char *next;
2826  unsigned long n = strtoul(buf, &next, 10);
2827  int save_errno = errno;
2828  end = next;
2829  if (end == buf) {
2830  return 0;
2831 
2832  } else if (end[0] != '\0') {
2833  return 0;
2834 
2835  } else if (save_errno == ERANGE) {
2836  silent_cerr("Modal(" << GetLabel() << "): "
2837  "warning, private data mode index "
2838  << std::string(buf, end - buf)
2839  << " overflows" << std::endl);
2840  return 0;
2841  }
2842 
2843  if (bIdx) {
2844  if (n > NModes) {
2845  return 0;
2846  }
2847 
2848  } else {
2849  std::vector<unsigned int>::const_iterator iv
2850  = std::find(uModeNumber.begin(), uModeNumber.end(), n);
2851  if (iv == uModeNumber.end()) {
2852  return 0;
2853  }
2854 
2855  n = iv - uModeNumber.begin() + 1;
2856  }
2857 
2858  return p*NModes + n;
2859  }
2860 
2861  end = std::strchr(s, ',');
2862  if (end == 0) {
2863  return 0;
2864  }
2865 
2866  std::string FEMLabel(s, end - s);
2867 
2868  end++;
2869  if (end[0] == '-') {
2870  return 0;
2871  }
2872 
2873  char *next;
2874  errno = 0;
2875  unsigned int index = strtoul(end, &next, 10);
2876  int save_errno = errno;
2877  if (next == end || next[0] != ']') {
2878  return 0;
2879 
2880  } else if (save_errno == ERANGE) {
2881  silent_cerr("Modal(" << GetLabel() << "): "
2882  "warning, private data FEM node component "
2883  << std::string(end, next - end)
2884  << " overflows" << std::endl);
2885  return 0;
2886  }
2887 
2888  if (index == 0 || index > 3) {
2889  return 0;
2890  }
2891 
2892  std::vector<std::string>::const_iterator ii = find(IdFEMNodes.begin(), IdFEMNodes.end(), FEMLabel);
2893  if (ii == IdFEMNodes.end()) {
2894  return 0;
2895  }
2896  unsigned int i = ii - IdFEMNodes.begin();
2897 
2898  return 3*NModes + 18*i + (what == 'w' ? 3 : 0) + 6*p + index;
2899 }
2900 
2901 
2902 doublereal
2903 Modal::dGetPrivData(unsigned int i) const
2904 {
2905  ASSERT(i > 0 && i < iGetNumPrivData());
2906 
2907  if (i <= NModes) {
2908  return a(i);
2909  }
2910 
2911  i -= NModes;
2912  if (i <= NModes) {
2913  return b(i);
2914  }
2915 
2916  i -= NModes;
2917  if (i <= NModes) {
2918  return bPrime(i);
2919  }
2920 
2921  i -= NModes;
2922  unsigned int n = (i - 1) / 18;
2923  unsigned int index = (i - 1) % 18 + 1;
2924  unsigned int p = (index - 1) / 6;
2925  unsigned int c = (index - 1) % 6 + 1;
2926  unsigned int w = (c - 1) / 3;
2927  if (w) {
2928  c -= 3;
2929  }
2930 
2931  switch (p) {
2932  case 0:
2933  if (w) {
2934  // TODO: orientation, somehow?
2935  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
2936 
2937  } else {
2938  // x == 0, R == eye otherwise
2939  if (pModalNode) {
2940  R = pModalNode->GetRCurr();
2941  x = pModalNode->GetXCurr();
2942  }
2943  Vec3 Xn(pXYZFEMNodes->GetVec(n + 1));
2944  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
2945  integer iOffset = (jMode - 1)*NFEMNodes + n + 1;
2946  Xn += pModeShapest->GetVec(iOffset)*a(jMode);
2947  }
2948 
2949  Vec3 X(x + R*Xn);
2950  return X(c);
2951  }
2952  break;
2953 
2954  case 1:
2955  if (w) {
2956  if (pModalNode) {
2957  R = pModalNode->GetRCurr();
2958  }
2959  Vec3 Wn(::Zero3);
2960  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
2961  integer iOffset = (jMode - 1)*NFEMNodes + n + 1;
2962  Wn += pModeShapesr->GetVec(iOffset)*b(jMode);
2963  }
2964 
2965  Vec3 W(R*Wn);
2966  if (pModalNode) {
2967  W += pModalNode->GetWCurr();
2968  }
2969  return W(c);
2970 
2971  } else {
2972  if (pModalNode) {
2973  R = pModalNode->GetRCurr();
2974  }
2975  Vec3 Vn(::Zero3);
2976  Vec3 Xn(pXYZFEMNodes->GetVec(n + 1));
2977  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
2978  integer iOffset = (jMode - 1)*NFEMNodes + n + 1;
2979  Vec3 v = pModeShapest->GetVec(iOffset);
2980  Vn += v*b(jMode);
2981  Xn += v*a(jMode);
2982  }
2983  Vec3 V(R*Vn);
2984  if (pModalNode) {
2985  V += pModalNode->GetWCurr().Cross(R*Xn);
2986  V += pModalNode->GetVCurr();
2987  }
2988  return V(c);
2989  }
2990  break;
2991 
2992  case 2:
2993  if (w) {
2994  if (pModalNode) {
2995  R = pModalNode->GetRCurr();
2996  }
2997  Vec3 WPn(::Zero3);
2998  Vec3 Wn(::Zero3);
2999  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
3000  integer iOffset = (jMode - 1)*NFEMNodes + n + 1;
3001  Vec3 v = pModeShapesr->GetVec(iOffset);
3002  WPn += v*bPrime(jMode);
3003  Wn += v*b(jMode);
3004  }
3005 
3006  Vec3 WP(R*WPn);
3007  if (pModalNode) {
3008  WP += pModalNode->GetWCurr().Cross(R*Wn);
3009  WP += pModalNode->GetWPCurr();
3010  }
3011  return WP(c);
3012 
3013  } else {
3014  if (pModalNode) {
3015  R = pModalNode->GetRCurr();
3016  }
3017  Vec3 XPPn(::Zero3);
3018  Vec3 XPn(::Zero3);
3019  Vec3 Xn(pXYZFEMNodes->GetVec(n + 1));
3020  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
3021  integer iOffset = (jMode - 1)*NFEMNodes + n + 1;
3022  Vec3 v = pModeShapest->GetVec(iOffset);
3023  XPPn += v*bPrime(jMode);
3024  XPn += v*b(jMode);
3025  Xn += v*a(jMode);
3026  }
3027  Vec3 XPP(R*XPPn);
3028  if (pModalNode) {
3029  const Vec3& W0 = pModalNode->GetWCurr();
3030  XPP += W0.Cross(R*(XPn*2.));
3031 
3032  Vec3 X(R*Xn);
3033  XPP += W0.Cross(W0.Cross(X));
3034  XPP += pModalNode->GetWPCurr().Cross(X);
3035  XPP += pModalNode->GetXPPCurr();
3036  }
3037  return XPP(c);
3038  }
3039  break;
3040  }
3041 
3042  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3043 }
3044 
3045 const Mat3xN&
3046 Modal::GetCurrFEMNodesPosition(void)
3047 {
3048  if (pCurrXYZ == 0) {
3049  SAFENEWWITHCONSTRUCTOR(pCurrXYZ, Mat3xN, Mat3xN(NFEMNodes, 0.));
3050  }
3051 
3052  if (pModalNode) {
3053  R = pModalNode->GetRCurr();
3054  x = pModalNode->GetXCurr();
3055  }
3056 
3057  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
3058  for (unsigned int iNode = 1; iNode <= NFEMNodes; iNode++) {
3059  for (unsigned int iCnt = 1; iCnt <= 3; iCnt++) {
3060  pCurrXYZ->Add(iCnt, iNode,
3061  pXYZFEMNodes->dGet(iCnt, iNode) + pModeShapest->dGet(iCnt, (jMode - 1)*NFEMNodes + iNode) * a(jMode) );
3062  }
3063  }
3064  }
3065 
3066  pCurrXYZ->LeftMult(R);
3067  for (unsigned int iNode = 1; iNode <= NFEMNodes; iNode++) {
3068  for (unsigned int iCnt = 1; iCnt <= 3; iCnt++) {
3069  pCurrXYZ->Add(iCnt, iNode, x(iCnt));
3070  }
3071  }
3072 
3073  return *pCurrXYZ;
3074 }
3075 
3076 const Mat3xN&
3077 Modal::GetCurrFEMNodesVelocity(void)
3078 {
3079  if (pCurrXYZ == 0) {
3080  SAFENEWWITHCONSTRUCTOR(pCurrXYZVel, Mat3xN, Mat3xN(NFEMNodes, 0.));
3081  }
3082 
3083  Vec3 w(::Zero3);
3084  Vec3 v(::Zero3);
3085 
3086  if (pModalNode) {
3087  R = pModalNode->GetRCurr();
3088  w = pModalNode->GetWCurr();
3089  v = pModalNode->GetVCurr();
3090  }
3091 
3092  Mat3x3 wWedge(MatCross, w);
3093 
3094  Mat3xN CurrXYZTmp(NFEMNodes, 0.);
3095  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
3096  for (unsigned int iNode = 1; iNode <= NFEMNodes; iNode++) {
3097  for (unsigned int iCnt = 1; iCnt <= 3; iCnt++) {
3098  doublereal d = pModeShapest->dGet(iCnt,(jMode - 1)*NFEMNodes + iNode);
3099  pCurrXYZVel->Add(iCnt, iNode, pXYZFEMNodes->dGet(iCnt,iNode) + d * a(jMode) );
3100  CurrXYZTmp.Add(iCnt, iNode,d * b(jMode) );
3101  }
3102  }
3103  }
3104  pCurrXYZVel->LeftMult(wWedge*R);
3105  CurrXYZTmp.LeftMult(R);
3106  for (unsigned int iNode = 1; iNode <= NFEMNodes; iNode++) {
3107  for (unsigned int iCnt = 1; iCnt <= 3; iCnt++) {
3108  pCurrXYZVel->Add(iCnt, iNode, CurrXYZTmp(iCnt, iNode) + v(iCnt));
3109  }
3110  }
3111 
3112  return *pCurrXYZVel;
3113 }
3114 
3115 /* from gravity.h */
3116 /* massa totale */
3117 doublereal
3118 Modal::dGetM(void) const
3119 {
3120  return dMass;
3121 }
3122 
3123 /* momento statico */
3124 Vec3
3125 Modal::GetS_int(void) const
3126 {
3127  if (pModalNode != 0) {
3128  x = pModalNode->GetXCurr();
3129  R = pModalNode->GetRCurr();
3130  }
3131 
3132  Vec3 STmp = Inv2;
3133  if (pInv3 != 0) {
3134  STmp += Inv3jaj;
3135  }
3136 
3137  return R*STmp + x*dMass;
3138 }
3139 
3140 /* momento d'inerzia */
3141 Mat3x3
3142 Modal::GetJ_int(void) const
3143 {
3144  if (pModalNode != 0) {
3145  x = pModalNode->GetXCurr();
3146  R = pModalNode->GetRCurr();
3147  }
3148 
3149  Vec3 STmp = Inv2;
3150  if (pInv3 != 0) {
3151  STmp += Inv3jaj;
3152  }
3153  Vec3 s(R*STmp);
3154 
3155  Mat3x3 JTmp = Inv7;
3156  if (pInv8 != 0) {
3157  JTmp += Inv8jaj.Symm2();
3158  if (pInv9 != 0) {
3159  JTmp -= Inv9jkajak;
3160  }
3161  }
3162 
3163  return R*JTmp.MulMT(R)
3164  - Mat3x3(MatCrossCross, x, x*dMass)
3165  - Mat3x3(MatCrossCross, x, s)
3166  - Mat3x3(MatCrossCross, s, x);
3167 }
3168 
3169 // momentum
3170 Vec3
3171 Modal::GetB_int(void) const
3172 {
3173  // FIXME: gross estimate
3174  if (pModalNode != 0) {
3175  return pModalNode->GetVCurr()*dMass;
3176  }
3177 
3178  return ::Zero3;
3179 }
3180 
3181 // momenta moment
3182 Vec3
3183 Modal::GetG_int(void) const
3184 {
3185  return ::Zero3;
3186 }
3187 
3188 Joint *
3189 ReadModal(DataManager* pDM,
3190  MBDynParser& HP,
3191  const DofOwner* pDO,
3192  unsigned int uLabel)
3193 {
3194  /* legge i dati d'ingresso e li passa al costruttore dell'elemento */
3195  Joint* pEl = 0;
3196 
3197  const ModalNode *pModalNode = 0;
3198  Vec3 X0(::Zero3);
3199  Mat3x3 R(::Eye3);
3200 
3201  /* If the modal element is clamped, a fixed position
3202  * and orientation of the reference point can be added */
3203  if (HP.IsKeyWord("clamped")) {
3204  if (HP.IsKeyWord("position")) {
3205  X0 = HP.GetPosAbs(::AbsRefFrame);
3206  }
3207 
3208  if (HP.IsKeyWord("orientation")) {
3209  R = HP.GetRotAbs(::AbsRefFrame);
3210  }
3211 
3212  /* otherwise a structural node of type "modal" must be given;
3213  * the "origin node" of the FEM model, if given, or the 0,0,0
3214  * coordinate point of the mesh will be put in the modal node */
3215  } else {
3216  unsigned int uNode = HP.GetInt();
3217 
3218  DEBUGCOUT("Linked to Modal Node: " << uNode << std::endl);
3219 
3220  /* verifica di esistenza del nodo */
3221  const StructNode* pTmpNode = pDM->pFindNode<const StructNode, Node::STRUCTURAL>(uNode);
3222 
3223  if (pTmpNode == 0) {
3224  silent_cerr("Modal(" << uLabel << "): "
3225  "StructuralNode(" << uNode << ") "
3226  "at line " << HP.GetLineData()
3227  << " not defined" << std::endl);
3228  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3229  }
3230 
3231  if (pTmpNode->GetStructNodeType() != StructNode::MODAL) {
3232  silent_cerr("Modal(" << uLabel << "): "
3233  "illegal type for "
3234  "StructuralNode(" << uNode << ") "
3235  "at line " << HP.GetLineData()
3236  << std::endl);
3237  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3238  }
3239  pModalNode = dynamic_cast<const ModalNode *>(pTmpNode);
3240  ASSERT(pModalNode != 0);
3241 
3242  if (pModalNode->pGetRBK() != 0) {
3243  silent_cerr("Modal(" << uLabel
3244  << "): StructuralNode(" << uNode
3245  << ") uses rigid body kinematics at line "
3246  << HP.GetLineData() << std::endl);
3247  throw ErrNotImplementedYet(MBDYN_EXCEPT_ARGS);
3248  }
3249  /* la posizione del nodo modale e' quella dell'origine del SdR
3250  * del corpo flessibile */
3251  X0 = pModalNode->GetXCurr();
3252 
3253  /* orientazione del corpo flessibile, data come orientazione
3254  * del nodo modale collegato */
3255  R = pModalNode->GetRCurr();
3256  }
3257 
3258  /* Legge i dati relativi al corpo flessibile */
3259  int tmpNModes = HP.GetInt(); /* numero di modi */
3260  if (tmpNModes <= 0) {
3261  silent_cerr("Modal(" << uLabel << "): "
3262  "illegal number of modes " << tmpNModes << " at line "
3263  << HP.GetLineData() << std::endl);
3264  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3265  }
3266  unsigned int NModes = (unsigned int)tmpNModes;
3267  unsigned int uLargestMode(0);
3268 
3269  std::vector<unsigned int> uModeNumber(NModes);
3270  if (HP.IsKeyWord("list")) {
3271  for (unsigned int iCnt = 0; iCnt < NModes; iCnt++) {
3272  int n = HP.GetInt();
3273 
3274  if (n <= 0) {
3275  silent_cerr("Modal(" << uLabel << "): "
3276  "illegal mode number "
3277  "for mode #" << iCnt + 1
3278  << " at line " << HP.GetLineData()
3279  << std::endl);
3280  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3281  }
3282 
3283  std::vector<unsigned int>::const_iterator
3284  iv = std::find(uModeNumber.begin(),
3285  uModeNumber.end(), (unsigned int)n);
3286  if (iv != uModeNumber.end()) {
3287  silent_cerr("Modal(" << uLabel << "): "
3288  "mode #" << iCnt + 1
3289  << " already defined "
3290  "at line " << HP.GetLineData()
3291  << std::endl);
3292  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3293  }
3294 
3295  if ((unsigned int)n > uLargestMode) {
3296  uLargestMode = (unsigned int)n;
3297  }
3298 
3299  uModeNumber[iCnt] = n;
3300  }
3301 
3302  } else {
3303  for (unsigned int iCnt = 1; iCnt <= NModes; iCnt++) {
3304  uModeNumber[iCnt - 1] = iCnt;
3305  }
3306  uLargestMode = NModes;
3307  }
3308 
3309  std::vector<doublereal> InitialValues(NModes, 0.);
3310  std::vector<doublereal> InitialValuesP(NModes, 0.);
3311  bool bInitialValues(false);
3312  if (HP.IsKeyWord("initial" "value")) {
3313  bInitialValues = true;
3314 
3315  if (HP.IsKeyWord("mode")) {
3316  int iCnt = 0;
3317  do {
3318  int n = HP.GetInt();
3319 
3320  if (n <= 0) {
3321  silent_cerr("Modal(" << uLabel << "): "
3322  "illegal mode number "
3323  "for initial value #" << iCnt + 1
3324  << " at line " << HP.GetLineData()
3325  << std::endl);
3326  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3327  }
3328 
3329  std::vector<unsigned int>::const_iterator
3330  iv = std::find(uModeNumber.begin(),
3331  uModeNumber.end(), (unsigned int)n);
3332  if (iv == uModeNumber.end()) {
3333  silent_cerr("Modal(" << uLabel << "): "
3334  "mode " << n << " not defined "
3335  "at line " << HP.GetLineData()
3336  << std::endl);
3337  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3338  }
3339 
3340  unsigned iIdx = iv - uModeNumber.begin();
3341  InitialValues[iIdx] = HP.GetReal();
3342  InitialValuesP[iIdx] = HP.GetReal();
3343 
3344  iCnt++;
3345  } while (HP.IsKeyWord("mode"));
3346 
3347  } else {
3348  for (unsigned int iCnt = 0; iCnt < NModes; iCnt++) {
3349  InitialValues[0] = HP.GetReal();
3350  InitialValuesP[0] = HP.GetReal();
3351  }
3352  }
3353  }
3354 
3355  /* numero di nodi FEM del modello */
3356  unsigned int NFEMNodes = unsigned(-1);
3357  if (!HP.IsKeyWord("from" "file")) {
3358  int tmpNFEMNodes = HP.GetInt();
3359  if (tmpNFEMNodes <= 0) {
3360  silent_cerr("Modal(" << uLabel << "): "
3361  "illegal number of FEM nodes " << tmpNFEMNodes
3362  << " at line " << HP.GetLineData() << std::endl);
3363  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3364  }
3365  NFEMNodes = unsigned(tmpNFEMNodes);
3366  }
3367 
3368 #ifdef MODAL_SCALE_DATA
3369  /* Legge gli eventuali fattori di scala per le masse nodali
3370  * (scale masses) e per i modi (scale modes) */
3371 
3372  /* NOTA: AL MOMENTO NON E' USATO */
3373  doublereal scalemasses(1.);
3374  doublereal scaleinertia(1.);
3375  doublereal scalemodes(1.);
3376 
3377  if (HP.IsKeyWord("scale" "masses")) {
3378  scalemasses = HP.GetReal();
3379  }
3380 
3381  if (HP.IsKeyWord("scale" "modes")) {
3382  scalemodes = HP.GetReal();
3383  }
3384 
3385  scaleinertia = scalemasses*(scalemodes*scalemodes);
3386 #endif /* MODAL_SCALE_DATA */
3387 
3388  /* Legge i coefficienti di smorzamento */
3389  enum {
3390  DAMPING_FROM_FILE = 0,
3391  DAMPING_NO,
3392  // DAMPING_PROPORTIONAL, // obsoleted
3393  DAMPING_RAYLEIGH,
3394  DAMPING_SINGLE_FACTOR,
3395  DAMPING_DIAG
3396  } eDamp(DAMPING_FROM_FILE);
3397  const char *sDamp[] = {
3398  "damping from file",
3399  "no damping",
3400  // "proportional damping", // obsoleted
3401  "Rayleigh damping",
3402  "single factor damping",
3403  "diag damping",
3404  NULL
3405  };
3406 
3407  doublereal damp_factor(0.), damp_coef_M(0.), damp_coef_K(0.);
3408  std::vector<doublereal> DampRatios;
3409 
3410  if (HP.IsKeyWord("no" "damping")) {
3411  eDamp = DAMPING_NO;
3412 
3413  } else if (HP.IsKeyWord("rayleigh" "damping")) {
3414  eDamp = DAMPING_RAYLEIGH;
3415  damp_coef_M = HP.GetReal();
3416  if (damp_coef_M < 0.) {
3417  silent_cerr("Modal(" << uLabel << "): "
3418  "warning, negative mass damping coefficient for \"Rayleigh damping\" "
3419  "at line " << HP.GetLineData() << std::endl);
3420  }
3421  damp_coef_K = HP.GetReal();
3422  if (damp_coef_K < 0.) {
3423  silent_cerr("Modal(" << uLabel << "): "
3424  "warning, negative stiffness damping coefficient for \"Rayleigh damping\" "
3425  "at line " << HP.GetLineData() << std::endl);
3426  }
3427 
3428  } else if (HP.IsKeyWord("single" "factor" "damping")) {
3429  eDamp = DAMPING_SINGLE_FACTOR;
3430  damp_factor = HP.GetReal();
3431  if (damp_factor < 0.) {
3432  silent_cerr("Modal(" << uLabel << "): "
3433  "warning, negative damping factor for \"single factor damping\" "
3434  "at line " << HP.GetLineData() << std::endl);
3435  }
3436 
3437  } else if (HP.IsKeyWord("proportional" "damping")) {
3438  silent_cerr("Modal(" << uLabel << "): "
3439  "warning, \"proportional damping\" is deprecated; "
3440  "use \"single factor damping\" with the desired damping factor instead "
3441  "at line " << HP.GetLineData() << std::endl);
3442  // NOTE: "proportional damping" deprecated, replaced by "single factor damping"
3443  eDamp = DAMPING_SINGLE_FACTOR;
3444  damp_factor = HP.GetReal();
3445  if (damp_factor < 0.) {
3446  silent_cerr("Modal(" << uLabel << "): "
3447  "warning, negative damping factor for \"proportional damping\" "
3448  "at line " << HP.GetLineData() << std::endl);
3449  }
3450 
3451  } else if (HP.IsKeyWord("diag" "damping")) {
3452  eDamp = DAMPING_DIAG;
3453  DampRatios.resize(NModes);
3454  fill(DampRatios.begin(), DampRatios.end(), 0.);
3455 
3456  if (HP.IsKeyWord("all")) {
3457  for (unsigned int iCnt = 0; iCnt < NModes; iCnt ++) {
3458  damp_factor = HP.GetReal();
3459  if (damp_factor < 0.) {
3460  silent_cerr("Modal(" << uLabel << "): "
3461  "warning, negative damping factor for \"diag damping\" "
3462  "of mode " << (iCnt + 1 ) << " "
3463  "at line " << HP.GetLineData() << std::endl);
3464  }
3465  DampRatios[iCnt] = damp_factor;
3466  }
3467 
3468  } else {
3469  int iDM = HP.GetInt();
3470  if (iDM <= 0 || unsigned(iDM) > NModes) {
3471  silent_cerr("invalid number of damped modes "
3472  "at line " << HP.GetLineData() <<std::endl);
3473  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3474  }
3475 
3476  std::vector<bool> gotit(NModes, false);
3477 
3478  for (unsigned int iCnt = 1; iCnt <= unsigned(iDM); iCnt ++) {
3479  integer iDampedMode = HP.GetInt();
3480  if (iDampedMode <= 0 || unsigned(iDampedMode) > NModes) {
3481  silent_cerr("invalid index " << iDampedMode
3482  << " for damped mode #" << iCnt
3483  << " of " << iDM
3484  << " at line " << HP.GetLineData() <<std::endl);
3485  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3486  }
3487 
3488  if (gotit[iDampedMode - 1]) {
3489  silent_cerr("damping already provided for mode " << iDampedMode
3490  << " (damped mode #" << iCnt
3491  << " of " << iDM
3492  << ") at line " << HP.GetLineData() <<std::endl);
3493  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3494 
3495  }
3496  gotit[iDampedMode - 1] = true;
3497  damp_factor = HP.GetReal();
3498  if (damp_factor < 0.) {
3499  silent_cerr("Modal(" << uLabel << "): "
3500  "warning, negative damping factor for \"diag damping\" "
3501  "of mode " << iDampedMode << " "
3502  "at line " << HP.GetLineData() << std::endl);
3503  }
3504  DampRatios[iDampedMode - 1] = damp_factor;
3505  }
3506  }
3507  } else if (HP.IsKeyWord("damping" "from" "file")) {
3508  eDamp = DAMPING_FROM_FILE;
3509  } else {
3510  silent_cout("Modal(" << uLabel << "): "
3511  "no damping is assumed at line "
3512  << HP.GetLineData() << " (deprecated)"
3513  << std::endl);
3514  }
3515 
3516  DEBUGCOUT("Number of Modes Imported : " << NModes << std::endl);
3517  DEBUGCOUT("Number of FEM Nodes Imported : " << NFEMNodes << std::endl);
3518  DEBUGCOUT("Origin of FEM Model : " << X0 << std::endl);
3519  DEBUGCOUT("Orientation of FEM Model : " << R << std::endl);
3520  /* DEBUGCOUT("Damping coefficient: "<< cdamp << std::endl); */
3521 
3522  doublereal dMass = 0; // total mass
3523  Vec3 XTmpIn(::Zero3); // center of mass location
3524  Vec3 STmp(::Zero3); // static moment
3525  Mat3x3 JTmp(::Zero3x3); // total inertia from input
3526  Mat3x3 JTmpIn(::Zero3x3); // total inertia after manipulation
3527  std::vector<doublereal> FEMMass; // nodal masses
3528  std::vector<Vec3> FEMJ; // nodal inertia (diagonal)
3529 
3530  Mat3xN *pModeShapest = 0; // displacement and rotation shapes
3531  Mat3xN *pModeShapesr = 0;
3532  Mat3xN PHIti(NModes, 0.); // i-th node shapes: 3*nmodes
3533  Mat3xN PHIri(NModes, 0.);
3534  Mat3xN *pXYZFEMNodes = 0; // pointer to nodal coords
3535  MatNxN *pGenMass = 0; // pointer to modal mass
3536  MatNxN *pGenStiff = 0; // pointer to modal stiffness
3537  MatNxN *pGenDamp = 0; // pointer to modal damping
3538 
3539  // inertia invariants
3540  Mat3xN *pInv3 = 0;
3541  Mat3xN *pInv4 = 0;
3542  Mat3xN *pInv5 = 0;
3543  Mat3xN *pInv8 = 0;
3544  Mat3xN *pInv9 = 0;
3545  Mat3xN *pInv10 = 0;
3546  Mat3xN *pInv11 = 0;
3547 
3548  // modal displacements and velocities
3549  VecN *a = 0;
3550  VecN *aP = 0;
3551 
3552  // FEM database file name
3553  const char *s = HP.GetFileName();
3554  if (s == 0) {
3555  silent_cerr("Modal(" << uLabel << "): unable to get "
3556  "modal data file name at line " << HP.GetLineData()
3557  << std::endl);
3558  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3559  }
3560 
3561  std::string sFileFEM(s);
3562 
3563  doublereal dMassThreshold = 0.;
3564  doublereal dDampingThreshold = 0.;
3565  doublereal dStiffnessThreshold = 0.;
3566  while (true) {
3567  if (HP.IsKeyWord("threshold")) {
3568  dStiffnessThreshold = HP.GetReal();
3569  if (dStiffnessThreshold < 0.) {
3570  silent_cerr("Modal(" << uLabel << "): "
3571  "invalid threshold " << dStiffnessThreshold
3572  << " at line " << HP.GetLineData()
3573  << std::endl);
3574  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3575  }
3576  dMassThreshold = dDampingThreshold = dStiffnessThreshold;
3577 
3578  } else if (HP.IsKeyWord("mass" "threshold")) {
3579  dMassThreshold = HP.GetReal();
3580  if (dMassThreshold < 0.) {
3581  silent_cerr("Modal(" << uLabel << "): "
3582  "invalid mass threshold " << dMassThreshold
3583  << " at line " << HP.GetLineData()
3584  << std::endl);
3585  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3586  }
3587 
3588  } else if (HP.IsKeyWord("damping" "threshold")) {
3589  dDampingThreshold = HP.GetReal();
3590  if (dDampingThreshold < 0.) {
3591  silent_cerr("Modal(" << uLabel << "): "
3592  "invalid damping threshold " << dDampingThreshold
3593  << " at line " << HP.GetLineData()
3594  << std::endl);
3595  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3596  }
3597 
3598  } else if (HP.IsKeyWord("stiffness" "threshold")) {
3599  dStiffnessThreshold = HP.GetReal();
3600  if (dStiffnessThreshold < 0.) {
3601  silent_cerr("Modal(" << uLabel << "): "
3602  "invalid stiffness threshold " << dStiffnessThreshold
3603  << " at line " << HP.GetLineData()
3604  << std::endl);
3605  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3606  }
3607 
3608  } else {
3609  break;
3610  }
3611  }
3612 
3613  /* loop for binary keywords */
3614  bool bCreateBinary = false;
3615  bool bUseBinary = false;
3616  bool bUpdateBinary = false;
3617  while (true) {
3618  if (HP.IsKeyWord("create" "binary")) {
3619  bCreateBinary = true;
3620 
3621  } else if (HP.IsKeyWord("use" "binary")) {
3622  bUseBinary = true;
3623 
3624  } else if (HP.IsKeyWord("update" "binary")) {
3625  bUpdateBinary = true;
3626 
3627  } else {
3628  break;
3629  }
3630  }
3631 
3632  std::string sEchoFileName;
3633  int iEchoPrecision(13);
3634  if (HP.IsKeyWord("echo")) {
3635  const char *s = HP.GetFileName();
3636  if (s == 0) {
3637  silent_cerr("Modal(" << uLabel << "): "
3638  "unable to parse echo file name at line " << HP.GetLineData()
3639  << std::endl);
3640  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3641  }
3642  sEchoFileName = s;
3643 
3644  if (HP.IsKeyWord("precision")) {
3645  iEchoPrecision = HP.GetInt();
3646  if (iEchoPrecision <= 0) {
3647  silent_cerr("Modal(" << uLabel << "): "
3648  "invalid precision at line " << HP.GetLineData()
3649  << std::endl);
3650  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3651  }
3652  }
3653  }
3654 
3655  /* stuff for binary FEM file handling */
3656  std::string sBinFileFEM;
3657  struct stat stFEM, stBIN;
3658  bool bReadFEM = true,
3659  bWriteBIN = false,
3660  bCheckBIN = false;
3661 
3662  if (stat(sFileFEM.c_str(), &stFEM) == -1) {
3663  int save_errno = errno;
3664  char *errmsg = strerror(save_errno);
3665 
3666  silent_cerr("Modal(" << uLabel << "): "
3667  "unable to stat(\"" << sFileFEM << "\") "
3668  "(" << save_errno << ": " << errmsg << ")"
3669  << std::endl);
3670  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3671  }
3672 
3673  if (bUseBinary || bCreateBinary || bUpdateBinary) {
3674  sBinFileFEM = sFileFEM + ".bin";
3675 
3676  if (stat(sBinFileFEM.c_str(), &stBIN) == -1) {
3677  int save_errno = errno;
3678  char *errmsg = strerror(save_errno);
3679 
3680  if (save_errno == ENOENT && (bCreateBinary || bUpdateBinary)) {
3681  silent_cerr("Modal(" << uLabel << "): "
3682  "creating binary file "
3683  "\"" << sBinFileFEM << "\" "
3684  "from file "
3685  "\"" << sFileFEM << "\""
3686  << std::endl);
3687 
3688  } else {
3689  silent_cerr("Modal(" << uLabel << "): "
3690  "unable to stat(\"" << sBinFileFEM << "\") "
3691  "(" << save_errno << ": " << errmsg << ")"
3692  << std::endl);
3693  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3694  }
3695 
3696  } else {
3697  /* can check bin */
3698  bCheckBIN = true;
3699  }
3700  }
3701 
3702  if (bUseBinary && bCheckBIN) {
3703  /* if timestamp of binary is later than of ASCII use binary */
3704  if (stBIN.st_mtime > stFEM.st_mtime) {
3705  bReadFEM = false;
3706 
3707  /* otherwise, if requested, update binary */
3708  } else {
3709  if (bUpdateBinary) {
3710  bWriteBIN = true;
3711 
3712  silent_cout("Modal(" << uLabel << "): "
3713  "binary database file \"" << sBinFileFEM << "\" "
3714  "older than text database file \"" << sFileFEM << "\"; "
3715  "updating"
3716  << std::endl);
3717 
3718  } else {
3719  silent_cerr("Modal(" << uLabel << "): "
3720  "warning, binary database file \"" << sBinFileFEM << "\" "
3721  "older than text database file \"" << sFileFEM << "\"; "
3722  "using text database file "
3723  "(enable \"update binary\" to refresh binary database file)"
3724  << std::endl);
3725  }
3726  }
3727 
3728  } else if (bCreateBinary || bUpdateBinary) {
3729  bWriteBIN = true;
3730  }
3731 
3732  /* alloca la memoria per le matrici necessarie a memorizzare i dati
3733  * relativi al corpo flessibile
3734  * nota: devo usare i puntatori perche' altrimenti non si riesce
3735  * a passarli al costruttore
3736  */
3737  SAFENEWWITHCONSTRUCTOR(pGenMass, MatNxN, MatNxN(NModes, 0.));
3738  SAFENEWWITHCONSTRUCTOR(pGenStiff, MatNxN, MatNxN(NModes, 0.));
3739  SAFENEWWITHCONSTRUCTOR(pGenDamp, MatNxN, MatNxN(NModes, 0.));
3740 
3741  SAFENEWWITHCONSTRUCTOR(a, VecN, VecN(NModes, 0.));
3742  SAFENEWWITHCONSTRUCTOR(aP, VecN, VecN(NModes, 0.));
3743 
3744  /* array contenente le label dei nodi FEM */
3745  std::vector<std::string> IdFEMNodes;
3746  std::vector<bool> bActiveModes;
3747 
3748  enum {
3749  MODAL_VERSION_UNKNOWN = 0,
3750 
3751  MODAL_VERSION_1 = 1,
3752  MODAL_VERSION_2 = 2,
3753  MODAL_VERSION_3 = 3, // after adding record 13 (generalized damping)
3754  MODAL_VERSION_4 = 4, // after making sizes portable
3755 
3756  MODAL_VERSION_LAST
3757  };
3758 
3759  /* NOTE: increment this each time the binary format changes! */
3760  const char magic[5] = "bmod";
3761  const uint32_t BinVersion = MODAL_VERSION_4;
3762 
3763  uint32_t currBinVersion = MODAL_VERSION_UNKNOWN;
3764  char checkPoint;
3765  uint32_t NFEMNodesFEM = 0, NModesFEM = 0;
3766 
3767  bool bBuildInvariants = false;
3768 
3769  enum {
3770  // note: we use 1-based array
3771  MODAL_RECORD_1 = 1,
3772  MODAL_RECORD_2 = 2,
3773  MODAL_RECORD_3 = 3,
3774  MODAL_RECORD_4 = 4,
3775  MODAL_RECORD_5 = 5,
3776  MODAL_RECORD_6 = 6,
3777  MODAL_RECORD_7 = 7,
3778  MODAL_RECORD_8 = 8,
3779  MODAL_RECORD_9 = 9,
3780  MODAL_RECORD_10 = 10,
3781  MODAL_RECORD_11 = 11,
3782  MODAL_RECORD_12 = 12,
3783  MODAL_RECORD_13 = 13,
3784 
3785  // NOTE: do not exceed 127
3786 
3787  MODAL_LAST_RECORD,
3788 
3789  MODAL_END_OF_FILE = char(-1)
3790  };
3791 
3792  /* Note: to keep it readable, we use a base 1 array */
3793  bool bRecordGroup[MODAL_LAST_RECORD] = { false };
3794 
3795  // record 1: header
3796  // record 2: FEM node labels
3797  // record 3: initial modal amplitudes
3798  // record 4: initial modal amplitude rates
3799  // record 5: FEM node X
3800  // record 6: FEM node Y
3801  // record 7: FEM node Z
3802  // record 8: shapes
3803  // record 9: generalized mass matrix
3804  // record 10: generalized stiffness matrix
3805  // record 11: (optional) diagonal mass matrix
3806  // record 12: (optional) rigid body inertia
3807  // record 13: (optional) damping matrix
3808 
3809  std::string fname;
3810  if (bReadFEM) {
3811  /* apre il file con i dati del modello FEM */
3812  std::ifstream fdat(sFileFEM.c_str());
3813  if (!fdat) {
3814  silent_cerr("Modal(" << uLabel << "): "
3815  "unable to open file \"" << sFileFEM << "\""
3816  "at line " << HP.GetLineData() << std::endl);
3817  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3818  }
3819 
3820  /* don't leave behind a corrupted .bin file */
3821  try {
3822 
3823  std::ofstream fbin;
3824  if (bWriteBIN) {
3825  fbin.open(sBinFileFEM.c_str(), std::ios::binary | std::ios::trunc);
3826  if (!fbin) {
3827  silent_cerr("Modal(" << uLabel << "): "
3828  "unable to open file \"" << sBinFileFEM << "\""
3829  "at line " << HP.GetLineData() << std::endl);
3830  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3831  }
3832 
3833  fbin.write(&magic[0], sizeof(4));
3834  currBinVersion = BinVersion;
3835  fbin.write((const char *)&currBinVersion, sizeof(currBinVersion));
3836  }
3837 
3838  /* carica i dati relativi a coordinate nodali, massa, momenti statici
3839  * e d'inerzia, massa e rigidezza generalizzate dal file nomefile.
3840  */
3841 
3842  doublereal d;
3843  unsigned int NRejModes = 0;
3844  char str[BUFSIZ];
3845 
3846  while (fdat && !fdat.eof()) { /* parsing del file */
3847  fdat.getline(str, sizeof(str));
3848 
3849  if (strncmp("** END OF FILE", str, STRLENOF("** END OF FILE")) == 0) {
3850  break;
3851  }
3852 
3853  /* legge il primo blocco (HEADER) */
3854  if (strncmp("** RECORD GROUP ", str,
3855  STRLENOF("** RECORD GROUP ")) != 0) {
3856  continue;
3857  }
3858 
3859  char *next;
3860  long rg = std::strtol(&str[STRLENOF("** RECORD GROUP ")], &next, 10);
3861  if (next == &str[STRLENOF("** RECORD GROUP ")]) {
3862  silent_cerr("file=\"" << sFileFEM << "\": "
3863  "unable to parse \"RECORD GROUP\" number <" << &str[STRLENOF("** RECORD GROUP ")] << ">"
3864  << std::endl);
3865  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3866  }
3867 
3868  if (!bRecordGroup[MODAL_RECORD_1] && rg != MODAL_RECORD_1) {
3869  silent_cerr("file=\"" << sFileFEM << "\": "
3870  "\"RECORD GROUP 1\" not parsed yet; cannot parse \"RECORD GROUP " << rg << "\""
3871  << std::endl);
3872  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3873  }
3874 
3875  switch (rg) {
3876  case MODAL_RECORD_1: {
3877  if (bRecordGroup[MODAL_RECORD_1]) {
3878  silent_cerr("file=\"" << sFileFEM << "\": "
3879  "\"RECORD GROUP 1\" already parsed"
3880  << std::endl);
3881  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3882  }
3883 
3884  fdat.getline(str, sizeof(str));
3885 
3886  /* ignore REV by now */
3887  fdat >> str;
3888 
3889  /* FEM nodes number */
3890  fdat >> NFEMNodesFEM;
3891 
3892  /* add to modes number */
3893  fdat >> NModesFEM;
3894 
3895  unsigned int i;
3896  fdat >> i;
3897  NModesFEM += i;
3898  fdat >> i;
3899  NModesFEM += i;
3900 
3901  /* "rejected" modes (subtract from modes number) */
3902  fdat >> NRejModes;
3903  NModesFEM -= NRejModes;
3904 
3905  /* consistency checks */
3906  if (NFEMNodes == unsigned(-1)) {
3907  NFEMNodes = NFEMNodesFEM;
3908 
3909  } else if (NFEMNodes != NFEMNodesFEM) {
3910  silent_cerr("Modal(" << uLabel << "), "
3911  "file \"" << sFileFEM << "\": "
3912  "FEM nodes number " << NFEMNodes
3913  << " does not match node number "
3914  << NFEMNodesFEM
3915  << std::endl);
3916  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3917  }
3918 
3919  if (NModes > NModesFEM) {
3920  silent_cerr("Modal(" << uLabel << "), "
3921  "file '" << sFileFEM << "': "
3922  "number of requested modes "
3923  "(" << NModes << ") "
3924  "exceeds number of available "
3925  "modes (" << NModesFEM << ")"
3926  << std::endl);
3927  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3928 
3929  }
3930 
3931  if (NModes < NModesFEM) {
3932  silent_cout("Modal(" << uLabel << "), "
3933  "file '" << sFileFEM << "': "
3934  "using " << NModes
3935  << " of " << NModesFEM
3936  << " available modes"
3937  << std::endl);
3938  }
3939 
3940  if (uLargestMode > NModesFEM) {
3941  silent_cerr("Modal(" << uLabel << "), "
3942  "file '" << sFileFEM << "': "
3943  "largest requested mode "
3944  "(" << uLargestMode << ") "
3945  "exceeds available modes "
3946  "(" << NModesFEM << ")"
3947  << std::endl);
3948  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
3949  }
3950 
3951  if (!bActiveModes.empty()) {
3952  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3953  }
3954 
3955  IdFEMNodes.resize(NFEMNodes);
3956 
3957  SAFENEWWITHCONSTRUCTOR(pXYZFEMNodes, Mat3xN, Mat3xN(NFEMNodes, 0.));
3958  SAFENEWWITHCONSTRUCTOR(pModeShapest, Mat3xN, Mat3xN(NFEMNodes*NModes, 0.));
3959  SAFENEWWITHCONSTRUCTOR(pModeShapesr, Mat3xN, Mat3xN(NFEMNodes*NModes, 0.));
3960 
3961  bActiveModes.resize(NModesFEM + 1, false);
3962 
3963  for (unsigned int iCnt = 0; iCnt < NModes; iCnt++) {
3964  if (uModeNumber[iCnt] > NModesFEM) {
3965  silent_cerr("Modal(" << uLabel << "): "
3966  "mode " << uModeNumber[iCnt]
3967  << " is not available (max = "
3968  << NModesFEM << ")"
3969  << std::endl);
3970  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3971  }
3972  bActiveModes[uModeNumber[iCnt]] = true;
3973  }
3974 
3975  if (bWriteBIN) {
3976  checkPoint = MODAL_RECORD_1;
3977  fbin.write(&checkPoint, sizeof(checkPoint));
3978  fbin.write((const char *)&NFEMNodesFEM, sizeof(NFEMNodesFEM));
3979  fbin.write((const char *)&NModesFEM, sizeof(NModesFEM));
3980  }
3981 
3982  bRecordGroup[MODAL_RECORD_1] = true;
3983  } break;
3984 
3985  case MODAL_RECORD_2: {
3986  /* legge il secondo blocco (Id.nodi) */
3987  if (bRecordGroup[MODAL_RECORD_2]) {
3988  silent_cerr("file=\"" << sFileFEM << "\": "
3989  "\"RECORD GROUP 2\" already parsed"
3990  << std::endl);
3991  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
3992  }
3993 
3994  for (unsigned int iNode = 0; iNode < NFEMNodes; iNode++) {
3995  // NOTE: this precludes the possibility
3996  // to have blanks in node labels
3997  fdat >> IdFEMNodes[iNode];
3998  }
3999 
4000  if (bWriteBIN) {
4001  checkPoint = MODAL_RECORD_2;
4002  fbin.write(&checkPoint, sizeof(checkPoint));
4003  for (unsigned int iNode = 0; iNode < NFEMNodes; iNode++) {
4004  uint32_t len = IdFEMNodes[iNode].size();
4005  fbin.write((const char *)&len, sizeof(len));
4006  fbin.write((const char *)IdFEMNodes[iNode].c_str(), len);
4007  }
4008  }
4009 
4010  bRecordGroup[2] = true;
4011  } break;
4012 
4013  case MODAL_RECORD_3: {
4014  /* deformate iniziali dei modi */
4015  if (bRecordGroup[MODAL_RECORD_3]) {
4016  silent_cerr("file=\"" << sFileFEM << "\": "
4017  "\"RECORD GROUP 3\" already parsed"
4018  << std::endl);
4019  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4020  }
4021 
4022  unsigned int iCnt = 1;
4023 
4024  if (bActiveModes.empty()) {
4025  silent_cerr("Modal(" << uLabel << "): "
4026  "input file \"" << sFileFEM << "\" "
4027  "is bogus (RECORD GROUP 3)"
4028  << std::endl);
4029  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4030  }
4031 
4032  if (bWriteBIN) {
4033  checkPoint = MODAL_RECORD_3;
4034  fbin.write(&checkPoint, sizeof(checkPoint));
4035  }
4036 
4037  for (unsigned int jMode = 1; jMode <= NModesFEM; jMode++) {
4038  fdat >> d;
4039 
4040  if (bWriteBIN) {
4041  fbin.write((const char *)&d, sizeof(d));
4042  }
4043 
4044  if (!bActiveModes[jMode]) {
4045  continue;
4046  }
4047  a->Put(iCnt, d);
4048  iCnt++;
4049  }
4050 
4051  bRecordGroup[MODAL_RECORD_3] = true;
4052  } break;
4053 
4054  case MODAL_RECORD_4: {
4055  /* velocita' iniziali dei modi */
4056  if (bRecordGroup[MODAL_RECORD_4]) {
4057  silent_cerr("file=\"" << sFileFEM << "\": "
4058  "\"RECORD GROUP 4\" already parsed"
4059  << std::endl);
4060  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4061  }
4062 
4063  unsigned int iCnt = 1;
4064 
4065  if (bActiveModes.empty()) {
4066  silent_cerr("Modal(" << uLabel << "): "
4067  "input file \"" << sFileFEM << "\" "
4068  "is bogus (RECORD GROUP 4)"
4069  << std::endl);
4070  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4071  }
4072 
4073  if (bWriteBIN) {
4074  checkPoint = MODAL_RECORD_4;
4075  fbin.write(&checkPoint, sizeof(checkPoint));
4076  }
4077 
4078  for (unsigned int jMode = 1; jMode <= NModesFEM; jMode++) {
4079  fdat >> d;
4080 
4081  if (bWriteBIN) {
4082  fbin.write((const char *)&d, sizeof(d));
4083  }
4084 
4085  if (!bActiveModes[jMode]) {
4086  continue;
4087  }
4088  aP->Put(iCnt, d);
4089  iCnt++;
4090  }
4091 
4092  bRecordGroup[MODAL_RECORD_4] = true;
4093  } break;
4094 
4095  case MODAL_RECORD_5: {
4096  /* Coordinate X dei nodi */
4097  if (bRecordGroup[MODAL_RECORD_5]) {
4098  silent_cerr("file=\"" << sFileFEM << "\": "
4099  "\"RECORD GROUP 5\" already parsed"
4100  << std::endl);
4101  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4102  }
4103 
4104  if (bWriteBIN) {
4105  checkPoint = MODAL_RECORD_5;
4106  fbin.write(&checkPoint, sizeof(checkPoint));
4107  }
4108 
4109  for (unsigned int iNode = 1; iNode <= NFEMNodes; iNode++) {
4110  fdat >> d;
4111 
4112  if (bWriteBIN) {
4113  fbin.write((const char *)&d, sizeof(d));
4114  }
4115 
4116 #ifdef MODAL_SCALE_DATA
4117  d *= scalemodes;
4118 #endif /* MODAL_SCALE_DATA */
4119  pXYZFEMNodes->Put(1, iNode, d);
4120  }
4121 
4122  bRecordGroup[MODAL_RECORD_5] = true;
4123  } break;
4124 
4125  case MODAL_RECORD_6: {
4126  /* Coordinate Y dei nodi*/
4127  if (bRecordGroup[MODAL_RECORD_6]) {
4128  silent_cerr("file=\"" << sFileFEM << "\": "
4129  "\"RECORD GROUP 6\" already parsed"
4130  << std::endl);
4131  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4132  }
4133 
4134  if (bWriteBIN) {
4135  checkPoint = MODAL_RECORD_6;
4136  fbin.write(&checkPoint, sizeof(checkPoint));
4137  }
4138 
4139  for (unsigned int iNode = 1; iNode <= NFEMNodes; iNode++) {
4140  fdat >> d;
4141 
4142  if (bWriteBIN) {
4143  fbin.write((const char *)&d, sizeof(d));
4144  }
4145 
4146 #ifdef MODAL_SCALE_DATA
4147  d *= scalemodes;
4148 #endif /* MODAL_SCALE_DATA */
4149  pXYZFEMNodes->Put(2, iNode, d);
4150  }
4151 
4152  bRecordGroup[MODAL_RECORD_6] = true;
4153  } break;
4154 
4155  case MODAL_RECORD_7: {
4156  /* Coordinate Z dei nodi*/
4157  if (bRecordGroup[MODAL_RECORD_7]) {
4158  silent_cerr("file=\"" << sFileFEM << "\": "
4159  "\"RECORD GROUP 7\" already parsed"
4160  << std::endl);
4161  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4162  }
4163 
4164  if (bWriteBIN) {
4165  checkPoint = MODAL_RECORD_7;
4166  fbin.write(&checkPoint, sizeof(checkPoint));
4167  }
4168 
4169  for (unsigned int iNode = 1; iNode <= NFEMNodes; iNode++) {
4170  fdat >> d;
4171 
4172  if (bWriteBIN) {
4173  fbin.write((const char *)&d, sizeof(d));
4174  }
4175 
4176 #ifdef MODAL_SCALE_DATA
4177  d *= scalemodes;
4178 #endif /* MODAL_SCALE_DATA */
4179  pXYZFEMNodes->Put(3, iNode, d);
4180  }
4181 
4182  bRecordGroup[MODAL_RECORD_7] = true;
4183  } break;
4184 
4185  case MODAL_RECORD_8: {
4186  /* Forme modali */
4187  if (bRecordGroup[MODAL_RECORD_8]) {
4188  silent_cerr("file=\"" << sFileFEM << "\": "
4189  "\"RECORD GROUP 8\" already parsed"
4190  << std::endl);
4191  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4192  }
4193 
4194  if (bWriteBIN) {
4195  checkPoint = MODAL_RECORD_8;
4196  fbin.write(&checkPoint, sizeof(checkPoint));
4197  }
4198 
4199  // we assume rejected modes come first
4200  for (unsigned int jMode = 1; jMode <= NRejModes; jMode++) {
4201  // header
4202  fdat.getline(str, sizeof(str));
4203 
4204  silent_cout("Modal(" << uLabel << "): rejecting mode #" << jMode
4205  << " (" << str << ")" << std::endl);
4206 
4207  // mode
4208  for (unsigned int iNode = 1; iNode <= NFEMNodes; iNode++) {
4209  doublereal n[6];
4210 
4211  fdat >> n[0] >> n[1] >> n[2]
4212  >> n[3] >> n[4] >> n[5];
4213  }
4214 
4215  /* FIXME: siamo sicuri di avere
4216  * raggiunto '\n'? */
4217  fdat.getline(str, sizeof(str));
4218  }
4219 
4220  if (bActiveModes.empty()) {
4221  silent_cerr("Modal(" << uLabel << "): "
4222  "input file \"" << sFileFEM << "\" "
4223  "is bogus (RECORD GROUP 8)"
4224  << std::endl);
4225  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4226  }
4227 
4228  unsigned int iCnt = 1;
4229  for (unsigned int jMode = 1; jMode <= NModesFEM; jMode++) {
4230  fdat.getline(str, sizeof(str));
4231  if (str[0] != '\0' && strncmp("**", str, STRLENOF("**")) != 0)
4232  {
4233  if (NRejModes) {
4234  silent_cerr("Modal(" << uLabel << "): "
4235  "input file \"" << sFileFEM << "\""
4236  "is bogus (\"**\" expected before mode #" << jMode
4237  << " of " << NModesFEM << "; "
4238  "#" << jMode + NRejModes
4239  << " of " << NModesFEM + NRejModes
4240  << " including rejected)"
4241  << std::endl);
4242  } else {
4243  silent_cerr("Modal(" << uLabel << "): "
4244  "input file \"" << sFileFEM << "\""
4245  "is bogus (\"**\" expected before mode #" << jMode
4246  << " of " << NModesFEM << ")"
4247  << std::endl);
4248  }
4249  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4250  }
4251 
4252  for (unsigned int iNode = 1; iNode <= NFEMNodes; iNode++) {
4253  doublereal n[6];
4254 
4255  fdat >> n[0] >> n[1] >> n[2]
4256  >> n[3] >> n[4] >> n[5];
4257 
4258  if (bWriteBIN) {
4259  fbin.write((const char *)n, sizeof(n));
4260  }
4261 
4262  if (!bActiveModes[jMode]) {
4263  continue;
4264  }
4265 
4266 #ifdef MODAL_SCALE_DATA
4267  n[0] *= scalemodes;
4268  n[1] *= scalemodes;
4269  n[2] *= scalemodes;
4270 #endif /* MODAL_SCALE_DATA */
4271  pModeShapest->PutVec((iCnt - 1)*NFEMNodes + iNode, Vec3(&n[0]));
4272  pModeShapesr->PutVec((iCnt - 1)*NFEMNodes + iNode, Vec3(&n[3]));
4273  }
4274 
4275  if (bActiveModes[jMode]) {
4276  iCnt++;
4277  }
4278  fdat.getline(str, sizeof(str));
4279  }
4280 
4281  bRecordGroup[MODAL_RECORD_8] = true;
4282  } break;
4283 
4284  case MODAL_RECORD_9: {
4285  /* Matrice di massa modale */
4286  if (bRecordGroup[MODAL_RECORD_9]) {
4287  silent_cerr("file=\"" << sFileFEM << "\": "
4288  "\"RECORD GROUP 9\" already parsed"
4289  << std::endl);
4290  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4291  }
4292 
4293  if (bActiveModes.empty()) {
4294  silent_cerr("Modal(" << uLabel << "): "
4295  "input file \"" << sFileFEM << "\" "
4296  "is bogus (RECORD GROUP 9)"
4297  << std::endl);
4298  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4299  }
4300 
4301  if (bWriteBIN) {
4302  checkPoint = MODAL_RECORD_9;
4303  fbin.write(&checkPoint, sizeof(checkPoint));
4304  }
4305 
4306  unsigned int iCnt = 1;
4307  for (unsigned int jMode = 1; jMode <= NModesFEM; jMode++) {
4308  unsigned int jCnt = 1;
4309 
4310  for (unsigned int kMode = 1; kMode <= NModesFEM; kMode++) {
4311  fdat >> d;
4312 
4313  if (dMassThreshold > 0.0 && fabs(d) < dMassThreshold) {
4314  d = 0.0;
4315  }
4316 
4317  if (bWriteBIN) {
4318  fbin.write((const char *)&d, sizeof(d));
4319  }
4320 
4321  if (!bActiveModes[jMode] || !bActiveModes[kMode]) {
4322  continue;
4323  }
4324  pGenMass->Put(iCnt, jCnt, d);
4325  jCnt++;
4326  }
4327 
4328  if (bActiveModes[jMode]) {
4329  iCnt++;
4330  }
4331  }
4332 
4333  bRecordGroup[MODAL_RECORD_9] = true;
4334  } break;
4335 
4336  case MODAL_RECORD_10: {
4337  /* Matrice di rigidezza modale */
4338  if (bRecordGroup[MODAL_RECORD_10]) {
4339  silent_cerr("file=\"" << sFileFEM << "\": "
4340  "\"RECORD GROUP 10\" already parsed"
4341  << std::endl);
4342  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4343  }
4344 
4345  if (bActiveModes.empty()) {
4346  silent_cerr("Modal(" << uLabel << "): "
4347  "input file \"" << sFileFEM << "\" "
4348  "is bogus (RECORD GROUP 10)"
4349  << std::endl);
4350  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4351  }
4352 
4353  if (bWriteBIN) {
4354  checkPoint = MODAL_RECORD_10;
4355  fbin.write(&checkPoint, sizeof(checkPoint));
4356  }
4357 
4358  unsigned int iCnt = 1;
4359  for (unsigned int jMode = 1; jMode <= NModesFEM; jMode++) {
4360  unsigned int jCnt = 1;
4361 
4362  for (unsigned int kMode = 1; kMode <= NModesFEM; kMode++) {
4363  fdat >> d;
4364 
4365  if (dStiffnessThreshold > 0.0 && fabs(d) < dStiffnessThreshold) {
4366  d = 0.0;
4367  }
4368 
4369  if (bWriteBIN) {
4370  fbin.write((const char *)&d, sizeof(d));
4371  }
4372 
4373  if (!bActiveModes[jMode] || !bActiveModes[kMode]) {
4374  continue;
4375  }
4376  pGenStiff->Put(iCnt, jCnt, d);
4377  jCnt++;
4378  }
4379 
4380  if (bActiveModes[jMode]) {
4381  iCnt++;
4382  }
4383  }
4384 
4385  bRecordGroup[MODAL_RECORD_10] = true;
4386  } break;
4387 
4388  case MODAL_RECORD_11: {
4389  /* Lumped Masses */
4390  if (bRecordGroup[MODAL_RECORD_11]) {
4391  silent_cerr("file=\"" << sFileFEM << "\": "
4392  "\"RECORD GROUP 11\" already parsed"
4393  << std::endl);
4394  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4395  }
4396 
4397  if (bWriteBIN) {
4398  checkPoint = MODAL_RECORD_11;
4399  fbin.write(&checkPoint, sizeof(checkPoint));
4400  }
4401 
4402  FEMMass.resize(NFEMNodes);
4403  FEMJ.resize(NFEMNodes);
4404 
4405  for (unsigned int iNode = 1; iNode <= NFEMNodes; iNode++) {
4406  doublereal tmpmass = 0.;
4407  for (unsigned int jCnt = 1; jCnt <= 6; jCnt++) {
4408  fdat >> d;
4409 
4410  if (bWriteBIN) {
4411  fbin.write((const char *)&d, sizeof(d));
4412  }
4413 
4414  switch (jCnt) {
4415  case 1:
4416  tmpmass = d;
4417 #ifdef MODAL_SCALE_DATA
4418  d *= scalemass;
4419 #endif /* MODAL_SCALE_DATA */
4420  FEMMass[iNode - 1] = d;
4421  break;
4422 
4423  case 2:
4424  case 3:
4425  if (d != tmpmass) {
4426  silent_cout("Warning: component(" << jCnt << ") "
4427  "of FEM node(" << iNode << ") mass "
4428  "differs from component(1)=" << tmpmass << std::endl);
4429  }
4430  break;
4431 
4432  case 4:
4433  case 5:
4434  case 6:
4435 #ifdef MODAL_SCALE_DATA
4436  d *= scaleinertia;
4437 #endif /* MODAL_SCALE_DATA */
4438  FEMJ[iNode - 1](jCnt - 3) = d;
4439  break;
4440  }
4441  }
4442  }
4443 
4444  bBuildInvariants = true;
4445 
4446  bRecordGroup[MODAL_RECORD_11] = true;
4447  } break;
4448 
4449  case MODAL_RECORD_12: {
4450  /* Rigid body inertia */
4451  if (bRecordGroup[MODAL_RECORD_12]) {
4452  silent_cerr("file=\"" << sFileFEM << "\": "
4453  "\"RECORD GROUP 12\" already parsed"
4454  << std::endl);
4455  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4456  }
4457 
4458  if (bWriteBIN) {
4459  checkPoint = MODAL_RECORD_12;
4460  fbin.write(&checkPoint, sizeof(checkPoint));
4461  }
4462 
4463  fdat >> d;
4464 
4465  if (bWriteBIN) {
4466  fbin.write((const char *)&d, sizeof(d));
4467  }
4468 
4469  dMass = d;
4470 
4471  // NOTE: the CM location is read and temporarily stored in STmp
4472  // later, it will be multiplied by the mass
4473  for (int iRow = 1; iRow <= 3; iRow++) {
4474  fdat >> d;
4475 
4476  if (bWriteBIN) {
4477  fbin.write((const char *)&d, sizeof(d));
4478  }
4479 
4480  XTmpIn(iRow) = d;
4481  }
4482 
4483  // here JTmp is with respect to the center of mass
4484  for (int iRow = 1; iRow <= 3; iRow++) {
4485  for (int iCol = 1; iCol <= 3; iCol++) {
4486  fdat >> d;
4487 
4488  if (bWriteBIN) {
4489  fbin.write((const char *)&d, sizeof(d));
4490  }
4491 
4492  JTmpIn(iRow, iCol) = d;
4493  }
4494  }
4495 
4496  bRecordGroup[MODAL_RECORD_12] = true;
4497  } break;
4498 
4499  case MODAL_RECORD_13: {
4500  /* Matrice di smorzamento modale */
4501  if (bRecordGroup[MODAL_RECORD_13]) {
4502  silent_cerr("file=\"" << sFileFEM << "\": "
4503  "\"RECORD GROUP 13\" already parsed"
4504  << std::endl);
4505  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4506  }
4507 
4508  if (bActiveModes.empty()) {
4509  silent_cerr("Modal(" << uLabel << "): "
4510  "input file \"" << sFileFEM << "\" "
4511  "is bogus (RECORD GROUP 13)"
4512  << std::endl);
4513  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4514  }
4515 
4516  if (bWriteBIN) {
4517  checkPoint = MODAL_RECORD_13;
4518  fbin.write(&checkPoint, sizeof(checkPoint));
4519  }
4520 
4521  unsigned int iCnt = 1;
4522  for (unsigned int jMode = 1; jMode <= NModesFEM; jMode++) {
4523  unsigned int jCnt = 1;
4524 
4525  for (unsigned int kMode = 1; kMode <= NModesFEM; kMode++) {
4526  fdat >> d;
4527 
4528  if (dDampingThreshold > 0.0 && fabs(d) < dDampingThreshold) {
4529  d = 0.0;
4530  }
4531 
4532  if (bWriteBIN) {
4533  fbin.write((const char *)&d, sizeof(d));
4534  }
4535 
4536  if (!bActiveModes[jMode] || !bActiveModes[kMode]) {
4537  continue;
4538  }
4539 
4540  if (eDamp == DAMPING_FROM_FILE) {
4541  pGenDamp->Put(iCnt, jCnt, d);
4542  }
4543 
4544  jCnt++;
4545  }
4546 
4547  if (bActiveModes[jMode]) {
4548  iCnt++;
4549  }
4550  }
4551 
4552  bRecordGroup[MODAL_RECORD_13] = true;
4553  } break;
4554 
4555  default:
4556  silent_cerr("file=\"" << sFileFEM << "\": "
4557  "\"RECORD GROUP " << rg << "\" unhandled"
4558  << std::endl);
4559  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4560  }
4561  }
4562 
4563  fdat.close();
4564  if (bWriteBIN) {
4565  checkPoint = MODAL_END_OF_FILE;
4566  fbin.write(&checkPoint, sizeof(checkPoint));
4567  fbin.close();
4568  }
4569 
4570  /* unlink binary file if create/update failed */
4571  } catch (...) {
4572  if (bWriteBIN) {
4573  // NOTE: might be worth leaving in place
4574  // for debugging purposes
4575  (void)unlink(sBinFileFEM.c_str());
4576  }
4577  throw;
4578  }
4579 
4580  fname = sFileFEM;
4581 
4582  } else {
4583  std::ifstream fbin(sBinFileFEM.c_str(), std::ios::binary);
4584  if (!fbin) {
4585  silent_cerr("Modal(" << uLabel << "): "
4586  "unable to open file \"" << sBinFileFEM << "\""
4587  "at line " << HP.GetLineData() << std::endl);
4588  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
4589  }
4590  silent_cout("Modal(" << uLabel << "): "
4591  "reading flexible body data from file "
4592  "\"" << sBinFileFEM << "\"" << std::endl);
4593 
4594  /* check version */
4595  // NOTE: any time the format of the binary file changes,
4596  // the BinVersion should be incremented.
4597  // When reading the binary file, the version of the file
4598  // is compared with that of the code. If they do not match:
4599  // if the file is newer than the code, abort.
4600  // if the file is older than the code, try to deal with it
4601  // as much as possible
4602  //
4603  // History:
4604  //
4605  // 1: first implementation
4606  //
4607  // 2: July 2008, at LaMSID
4608  // - string FEM labels
4609  // - 3 & 4 optional
4610  // - 11 & 12 optional and no longer mutually exclusive
4611  // - -1 as the last checkpoint
4612  // - version 1 tolerated
4613  // 3: November 2012
4614  // - record 13, generalized damping matrix
4615  // 4: December 2012; incompatible with previous ones
4616  // - bin file portable (fixed size types, magic signature)
4617  char currMagic[5] = { 0 };
4618  fbin.read((char *)&currMagic, sizeof(4));
4619  if (memcmp(magic, currMagic, 4) != 0) {
4620  silent_cerr("Modal(" << uLabel << "): "
4621  "file \"" << sBinFileFEM << "\" "
4622  "unexpected signature" << std::endl);
4623  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4624  }
4625 
4626  fbin.read((char *)&currBinVersion, sizeof(currBinVersion));
4627  if (currBinVersion > BinVersion) {
4628  silent_cerr("Modal(" << uLabel << "): "
4629  "file \"" << sBinFileFEM << "\" "
4630  "version " << currBinVersion << " unsupported" << std::endl);
4631  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4632 
4633  } else if (currBinVersion < BinVersion) {
4634  silent_cout("Modal(" << uLabel << "): "
4635  "file \"" << sBinFileFEM << "\" "
4636  "version " << currBinVersion << " is outdated; "
4637  "trying to read anyway..." << std::endl);
4638  }
4639 
4640  pedantic_cout("Modal(" << uLabel << "): "
4641  "binary version " << currBinVersion << std::endl);
4642 
4643  // NOTE: the binary file is expected to be in order;
4644  // however, if generated from a non-ordered textual file
4645  // it will be out of order... perhaps also the textual
4646  // file should always be ordered?
4647 
4648  /* legge il primo blocco (HEADER) */
4649  fbin.read(&checkPoint, sizeof(checkPoint));
4650  if (checkPoint != MODAL_RECORD_1) {
4651  silent_cerr("Modal(" << uLabel << "): "
4652  "file \"" << sBinFileFEM << "\" "
4653  "looks broken (expecting checkpoint 1)"
4654  << std::endl);
4655  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4656  }
4657 
4658  pedantic_cout("Modal(" << uLabel << "): "
4659  "reading block " << unsigned(checkPoint) << std::endl);
4660 
4661  fbin.read((char *)&NFEMNodesFEM, sizeof(NFEMNodesFEM));
4662  fbin.read((char *)&NModesFEM, sizeof(NModesFEM));
4663 
4664  /* consistency checks */
4665  if (NFEMNodes == unsigned(-1)) {
4666  NFEMNodes = NFEMNodesFEM;
4667 
4668  } else if (NFEMNodes != NFEMNodesFEM) {
4669  silent_cerr("Modal(" << uLabel << "), "
4670  "file \"" << sFileFEM << "\": "
4671  "FEM nodes number " << NFEMNodes
4672  << " does not match node number "
4673  << NFEMNodesFEM
4674  << std::endl);
4675  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
4676  }
4677 
4678  if (NModes != NModesFEM) {
4679  silent_cout("Modal(" << uLabel
4680  << "), file '" << sFileFEM
4681  << "': using " << NModes
4682  << " of " << NModesFEM
4683  << " modes" << std::endl);
4684  }
4685 
4686  if (!bActiveModes.empty()) {
4687  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4688  }
4689 
4690  IdFEMNodes.resize(NFEMNodes);
4691 
4692  SAFENEWWITHCONSTRUCTOR(pXYZFEMNodes, Mat3xN, Mat3xN(NFEMNodes, 0.));
4693  SAFENEWWITHCONSTRUCTOR(pModeShapest, Mat3xN, Mat3xN(NFEMNodes*NModes, 0.));
4694  SAFENEWWITHCONSTRUCTOR(pModeShapesr, Mat3xN, Mat3xN(NFEMNodes*NModes, 0.));
4695 
4696  bActiveModes.resize(NModesFEM + 1, false);
4697 
4698  for (unsigned int iCnt = 0; iCnt < NModes; iCnt++) {
4699  if (uModeNumber[iCnt] > NModesFEM) {
4700  silent_cerr("Modal(" << uLabel << "): "
4701  "mode " << uModeNumber[iCnt]
4702  << " is not available (max = "
4703  << NModesFEM << ")"
4704  << std::endl);
4705  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4706  }
4707  bActiveModes[uModeNumber[iCnt]] = true;
4708  }
4709 
4710  bRecordGroup[1] = true;
4711 
4712  for (;;) {
4713  fbin.read(&checkPoint, sizeof(checkPoint));
4714 
4715  if (checkPoint == MODAL_END_OF_FILE) {
4716  pedantic_cout("Modal(" << uLabel << "): "
4717  "end of binary file \"" << sBinFileFEM << "\""
4718  << std::endl);
4719  fbin.close();
4720  break;
4721  }
4722 
4723  if (checkPoint < MODAL_RECORD_1 || checkPoint >= MODAL_LAST_RECORD) {
4724  silent_cerr("Modal(" << uLabel << "): "
4725  "file \"" << sBinFileFEM << "\" "
4726  "looks broken (unknown block " << unsigned(checkPoint) << ")"
4727  << std::endl);
4728  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4729  }
4730 
4731  if (bRecordGroup[unsigned(checkPoint)]) {
4732  silent_cerr("Modal(" << uLabel << "): "
4733  "file \"" << sBinFileFEM << "\" "
4734  "looks broken (block " << unsigned(checkPoint) << " already parsed)"
4735  << std::endl);
4736  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4737  }
4738 
4739  pedantic_cout("Modal(" << uLabel << "): "
4740  "reading block " << unsigned(checkPoint)
4741  << " from file \"" << sBinFileFEM << "\"" << std::endl);
4742 
4743  switch (checkPoint) {
4744  case MODAL_RECORD_2:
4745  /* legge il secondo blocco (Id.nodi) */
4746  for (unsigned int iNode = 0; iNode < NFEMNodes; iNode++) {
4747  uint32_t len;
4748  fbin.read((char *)&len, sizeof(len));
4749  ASSERT(len > 0);
4750  IdFEMNodes[iNode].resize(len);
4751  fbin.read((char *)IdFEMNodes[iNode].c_str(), len);
4752  }
4753  break;
4754 
4755  case MODAL_RECORD_3:
4756  /* deformate iniziali dei modi */
4757  for (unsigned int iCnt = 1, jMode = 1; jMode <= NModesFEM; jMode++) {
4758  doublereal d;
4759 
4760  fbin.read((char *)&d, sizeof(d));
4761 
4762  if (!bActiveModes[jMode]) {
4763  continue;
4764  }
4765 
4766  a->Put(iCnt, d);
4767  iCnt++;
4768  }
4769  break;
4770 
4771  case MODAL_RECORD_4:
4772  /* velocita' iniziali dei modi */
4773  for (unsigned int iCnt = 1, jMode = 1; jMode <= NModesFEM; jMode++) {
4774  doublereal d;
4775 
4776  fbin.read((char *)&d, sizeof(d));
4777 
4778  if (!bActiveModes[jMode]) {
4779  continue;
4780  }
4781 
4782  aP->Put(iCnt, d);
4783  iCnt++;
4784  }
4785  break;
4786 
4787  case MODAL_RECORD_5:
4788  /* Coordinate X dei nodi */
4789  for (unsigned int iNode = 1; iNode <= NFEMNodes; iNode++) {
4790  doublereal d;
4791 
4792  fbin.read((char *)&d, sizeof(d));
4793 
4794 #ifdef MODAL_SCALE_DATA
4795  d *= scalemodes;
4796 #endif /* MODAL_SCALE_DATA */
4797 
4798  pXYZFEMNodes->Put(1, iNode, d);
4799  }
4800  break;
4801 
4802  case MODAL_RECORD_6:
4803  /* Coordinate Y dei nodi*/
4804  for (unsigned int iNode = 1; iNode <= NFEMNodes; iNode++) {
4805  doublereal d;
4806 
4807  fbin.read((char *)&d, sizeof(d));
4808 
4809 #ifdef MODAL_SCALE_DATA
4810  d *= scalemodes;
4811 #endif /* MODAL_SCALE_DATA */
4812 
4813  pXYZFEMNodes->Put(2, iNode, d);
4814  }
4815  break;
4816 
4817  case MODAL_RECORD_7:
4818  /* Coordinate Z dei nodi*/
4819  for (unsigned int iNode = 1; iNode <= NFEMNodes; iNode++) {
4820  doublereal d;
4821 
4822  fbin.read((char *)&d, sizeof(d));
4823 
4824 #ifdef MODAL_SCALE_DATA
4825  d *= scalemodes;
4826 #endif /* MODAL_SCALE_DATA */
4827 
4828  pXYZFEMNodes->Put(3, iNode, d);
4829  }
4830  break;
4831 
4832  case MODAL_RECORD_8:
4833  /* Forme modali */
4834  for (unsigned int iCnt = 1, jMode = 1; jMode <= NModesFEM; jMode++) {
4835  for (unsigned int iNode = 1; iNode <= NFEMNodes; iNode++) {
4836  doublereal n[6];
4837 
4838  fbin.read((char *)n, sizeof(n));
4839 
4840  if (!bActiveModes[jMode]) {
4841  continue;
4842  }
4843 
4844 #ifdef MODAL_SCALE_DATA
4845  n[0] *= scalemodes;
4846  n[1] *= scalemodes;
4847  n[2] *= scalemodes;
4848 #endif /* MODAL_SCALE_DATA */
4849  pModeShapest->PutVec((iCnt - 1)*NFEMNodes + iNode, Vec3(&n[0]));
4850  pModeShapesr->PutVec((iCnt - 1)*NFEMNodes + iNode, Vec3(&n[3]));
4851  }
4852 
4853  if (bActiveModes[jMode]) {
4854  iCnt++;
4855  }
4856  }
4857  break;
4858 
4859  case MODAL_RECORD_9:
4860  /* Matrice di massa modale */
4861  for (unsigned int iCnt = 1, jMode = 1; jMode <= NModesFEM; jMode++) {
4862  unsigned int jCnt = 1;
4863 
4864  for (unsigned int kMode = 1; kMode <= NModesFEM; kMode++) {
4865  doublereal d;
4866 
4867  fbin.read((char *)&d, sizeof(d));
4868 
4869  if (!bActiveModes[jMode] || !bActiveModes[kMode]) {
4870  continue;
4871  }
4872  pGenMass->Put(iCnt, jCnt, d);
4873  jCnt++;
4874  }
4875 
4876  if (bActiveModes[jMode]) {
4877  iCnt++;
4878  }
4879  }
4880  break;
4881 
4882  case MODAL_RECORD_10:
4883  /* Matrice di rigidezza modale */
4884  for (unsigned int iCnt = 1, jMode = 1; jMode <= NModesFEM; jMode++) {
4885  unsigned int jCnt = 1;
4886 
4887  for (unsigned int kMode = 1; kMode <= NModesFEM; kMode++) {
4888  doublereal d;
4889 
4890  fbin.read((char *)&d, sizeof(d));
4891 
4892  if (!bActiveModes[jMode] || !bActiveModes[kMode]) {
4893  continue;
4894  }
4895  pGenStiff->Put(iCnt, jCnt, d);
4896  jCnt++;
4897  }
4898 
4899  if (bActiveModes[jMode]) {
4900  iCnt++;
4901  }
4902  }
4903  break;
4904 
4905  case MODAL_RECORD_11:
4906  /* Lumped Masses */
4907  FEMMass.resize(NFEMNodes);
4908  FEMJ.resize(NFEMNodes);
4909 
4910  for (unsigned int iNode = 1; iNode <= NFEMNodes; iNode++) {
4911  for (unsigned int jCnt = 1; jCnt <= 6; jCnt++) {
4912  doublereal d;
4913 
4914  fbin.read((char *)&d, sizeof(d));
4915 
4916  switch (jCnt) {
4917  case 1:
4918 #ifdef MODAL_SCALE_DATA
4919  d *= scalemass;
4920 #endif /* MODAL_SCALE_DATA */
4921  FEMMass[iNode - 1] = d;
4922  break;
4923 
4924  case 4:
4925  case 5:
4926  case 6:
4927 #ifdef MODAL_SCALE_DATA
4928  d *= scaleinertia;
4929 #endif /* MODAL_SCALE_DATA */
4930  FEMJ[iNode - 1](jCnt - 3) = d;
4931  break;
4932  }
4933  }
4934  }
4935 
4936  bBuildInvariants = true;
4937  break;
4938 
4939  case MODAL_RECORD_12: {
4940  doublereal d;
4941  fbin.read((char *)&d, sizeof(d));
4942  dMass = d;
4943 
4944  // NOTE: the CM location is read and temporarily stored in XTmpIn
4945  // later, it will be multiplied by the mass
4946  for (int iRow = 1; iRow <= 3; iRow++) {
4947  fbin.read((char *)&d, sizeof(d));
4948 
4949  XTmpIn(iRow) = d;
4950  }
4951 
4952  // here JTmpIn is with respect to the center of mass
4953  for (int iRow = 1; iRow <= 3; iRow++) {
4954  for (int iCol = 1; iCol <= 3; iCol++) {
4955  fbin.read((char *)&d, sizeof(d));
4956 
4957  JTmpIn(iRow, iCol) = d;
4958  }
4959  }
4960  } break;
4961 
4962  case MODAL_RECORD_13:
4963  for (unsigned int iCnt = 1, jMode = 1; jMode <= NModesFEM; jMode++) {
4964  unsigned int jCnt = 1;
4965 
4966  for (unsigned int kMode = 1; kMode <= NModesFEM; kMode++) {
4967  doublereal d;
4968 
4969  fbin.read((char *)&d, sizeof(d));
4970 
4971  if (!bActiveModes[jMode] || !bActiveModes[kMode]) {
4972  continue;
4973  }
4974 
4975  if (eDamp == DAMPING_FROM_FILE) {
4976  pGenDamp->Put(iCnt, jCnt, d);
4977  }
4978 
4979  jCnt++;
4980  }
4981 
4982  if (bActiveModes[jMode]) {
4983  iCnt++;
4984  }
4985  }
4986  break;
4987 
4988  default:
4989  silent_cerr("Modal(" << uLabel << "): "
4990  "file \"" << sBinFileFEM << "\" "
4991  "looks broken (unknown block " << unsigned(checkPoint) << ")"
4992  << std::endl);
4993  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
4994  }
4995 
4996  bRecordGroup[unsigned(checkPoint)] = true;
4997  }
4998 
4999  fname = sBinFileFEM;
5000  }
5001 
5002  unsigned reqMR[] = {
5003  MODAL_RECORD_1,
5004  MODAL_RECORD_2,
5005  // 3 & 4 no longer required; explicit check is present when currBinVersion == 1
5006  MODAL_RECORD_5,
5007  MODAL_RECORD_6,
5008  MODAL_RECORD_7,
5009  MODAL_RECORD_8,
5010  MODAL_RECORD_9,
5011  MODAL_RECORD_10
5012  };
5013  bool bBailOut(false);
5014  for (unsigned iCnt = 0; iCnt < sizeof(reqMR)/sizeof(unsigned); iCnt++) {
5015  if (!bRecordGroup[reqMR[iCnt]]) {
5016  silent_cerr("Modal(" << uLabel << "): "
5017  "incomplete input file \"" << fname << "\", "
5018  "record group " << reqMR[iCnt] << " missing "
5019  "at line " << HP.GetLineData() << std::endl);
5020  bBailOut = true;
5021  }
5022  }
5023 
5024  if (bBailOut) {
5025  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
5026  }
5027 
5028  // JTmp is now referred to the origin
5029  JTmp = JTmpIn - Mat3x3(MatCrossCross, XTmpIn, XTmpIn*dMass);
5030 
5031  if (!sEchoFileName.empty()) {
5032  std::ofstream fecho(sEchoFileName.c_str());
5033 
5034  fecho.setf(std::ios::scientific);
5035  fecho.precision(iEchoPrecision);
5036 
5037  time_t t;
5038  time(&t);
5039 
5040  fecho
5041  << "** echo of file \"" << fname << "\" generated " << ctime(&t)
5042  << "** RECORD GROUP 1, HEADER" << std::endl
5043  << "** REVISION. NODES. NORMAL, ATTACHMENT, CONSTRAINT, REJECTED MODES." << std::endl
5044  << "VER" << currBinVersion << " " << NFEMNodes << " " << NModes << " 0 0 0" << std::endl
5045  << "**" << std::endl
5046  << "** RECORD GROUP 2, FINITE ELEMENT NODE LIST" << std::endl;
5047  for (unsigned r = 0; r <= (IdFEMNodes.size() - 1)/6; r++) {
5048  for (unsigned c = 0; c < std::min(6U, unsigned(IdFEMNodes.size() - 6*r)); c++) {
5049  fecho << IdFEMNodes[6*r + c] << " ";
5050  }
5051  fecho << std::endl;
5052  }
5053 
5054  fecho
5055  << "**" << std::endl
5056  << "** RECORD GROUP 3, INITIAL MODAL DISPLACEMENTS"<< std::endl;
5057  for (int r = 1; r <= a->iGetNumRows(); r++) {
5058  fecho << (*a)(r) << std::endl;
5059  }
5060 
5061  fecho
5062  << "**" << std::endl
5063  << "** RECORD GROUP 4, INITIAL MODAL VELOCITIES"<< std::endl;
5064  for (int r = 1; r <= aP->iGetNumRows(); r++) {
5065  fecho << (*aP)(r) << std::endl;
5066  }
5067 
5068  fecho
5069  << "**" << std::endl
5070  << "** RECORD GROUP 5, NODAL X COORDINATES" << std::endl;
5071  for (int r = 1; r <= pXYZFEMNodes->iGetNumCols(); r++) {
5072  fecho << (*pXYZFEMNodes)(1, r) << std::endl;
5073  }
5074 
5075  fecho
5076  << "**" << std::endl
5077  << "** RECORD GROUP 6, NODAL Y COORDINATES" << std::endl;
5078  for (int r = 1; r <= pXYZFEMNodes->iGetNumCols(); r++) {
5079  fecho << (*pXYZFEMNodes)(2, r) << std::endl;
5080  }
5081 
5082  fecho
5083  << "**" << std::endl
5084  << "** RECORD GROUP 7, NODAL Z COORDINATES" << std::endl;
5085  for (int r = 1; r <= pXYZFEMNodes->iGetNumCols(); r++) {
5086  fecho << (*pXYZFEMNodes)(3, r) << std::endl;
5087  }
5088 
5089  fecho
5090  << "**" << std::endl
5091  << "** RECORD GROUP 8, NON-ORTHOGONALIZED MODE SHAPES" << std::endl;
5092  for (unsigned m = 0; m < NModes; m++) {
5093  fecho
5094  << "** NORMAL MODE SHAPE # " << uModeNumber[m] << std::endl;
5095  for (unsigned n = 1; n <= NFEMNodes; n++) {
5096  fecho
5097  << (*pModeShapest)(1, m*NFEMNodes + n) << " "
5098  << (*pModeShapest)(2, m*NFEMNodes + n) << " "
5099  << (*pModeShapest)(3, m*NFEMNodes + n) << " "
5100  << (*pModeShapesr)(1, m*NFEMNodes + n) << " "
5101  << (*pModeShapesr)(2, m*NFEMNodes + n) << " "
5102  << (*pModeShapesr)(3, m*NFEMNodes + n) << std::endl;
5103  }
5104  }
5105 
5106  fecho
5107  << "**" << std::endl
5108  << "** RECORD GROUP 9, MODAL MASS MATRIX. COLUMN-MAJOR FORM" << std::endl;
5109  for (unsigned r = 1; r <= NModes; r++) {
5110  for (unsigned c = 1; c <= NModes; c++) {
5111  fecho << (*pGenMass)(r, c) << " ";
5112  }
5113  fecho << std::endl;
5114  }
5115 
5116  fecho
5117  << "**" << std::endl
5118  << "** RECORD GROUP 10, MODAL STIFFNESS MATRIX. COLUMN-MAJOR FORM" << std::endl;
5119  for (unsigned r = 1; r <= NModes; r++) {
5120  for (unsigned c = 1; c <= NModes; c++) {
5121  fecho << (*pGenStiff)(r, c) << " ";
5122  }
5123  fecho << std::endl;
5124  }
5125 
5126  if (bBuildInvariants) {
5127  fecho
5128  << "**" << std::endl
5129  << "** RECORD GROUP 11, DIAGONAL OF LUMPED MASS MATRIX" << std::endl;
5130  for (unsigned r = 0; r < FEMMass.size(); r++) {
5131  fecho
5132  << FEMMass[r] << " " << FEMMass[r] << " " << FEMMass[r] << " "
5133  << FEMJ[r] << std::endl;
5134  }
5135  }
5136 
5137  if (bRecordGroup[MODAL_RECORD_12]) {
5138  fecho
5139  << "**" << std::endl
5140  << "** RECORD GROUP 12, GLOBAL INERTIA" << std::endl
5141  << dMass << std::endl
5142  << XTmpIn << std::endl,
5143  JTmpIn.Write(fecho, " ", "\n") << std::endl;
5144  }
5145 
5146  if (bRecordGroup[MODAL_RECORD_13]) {
5147  fecho
5148  << "**" << std::endl
5149  << "** RECORD GROUP 13, MODAL DAMPING MATRIX. COLUMN-MAJOR FORM" << std::endl;
5150  for (unsigned r = 1; r <= NModes; r++) {
5151  for (unsigned c = 1; c <= NModes; c++) {
5152  fecho << (*pGenDamp)(r, c) << " ";
5153  }
5154  fecho << std::endl;
5155  }
5156  }
5157  }
5158 
5159  /* lettura dati di vincolo:
5160  * l'utente specifica la label del nodo FEM e del nodo rigido
5161  * d'interfaccia.
5162  * L'orientamento del nodo FEM e' quello del nodo modale, la
5163  * posizione e' la somma di quella modale e di quella FEM */
5164 
5165  /* array contenente le forme modali dei nodi d'interfaccia */
5166  Mat3xN* pPHItStrNode = 0;
5167  Mat3xN* pPHIrStrNode = 0;
5168 
5169  /* traslazione origine delle coordinate */
5170  Vec3 Origin(::Zero3);
5171  bool bOrigin(false);
5172 
5173  if (HP.IsKeyWord("origin" "node")) {
5174  /* numero di nodi FEM del modello */
5175  std::string FEMOriginNode;
5176  if (HP.IsStringWithDelims()) {
5177  FEMOriginNode = HP.GetStringWithDelims();
5178 
5179  } else {
5180  pedantic_cerr("Modal(" << uLabel << "): "
5181  "origin node expected as string with delimiters"
5182  << std::endl);
5183  FEMOriginNode = HP.GetString("");
5184  }
5185 
5186  unsigned int iNode;
5187  for (iNode = 0; iNode < NFEMNodes; iNode++) {
5188  if (FEMOriginNode == IdFEMNodes[iNode]) {
5189  break;
5190  }
5191  }
5192 
5193  if (iNode == NFEMNodes) {
5194  silent_cerr("Modal(" << uLabel << "): "
5195  "FEM node \"" << FEMOriginNode << "\""
5196  << " at line " << HP.GetLineData()
5197  << " not defined " << std::endl);
5198  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
5199  }
5200 
5201  iNode++;
5202  Origin = pXYZFEMNodes->GetVec(iNode);
5203 
5204  pedantic_cout("Modal(" << uLabel << "): origin x={" << Origin << "}" << std::endl);
5205  bOrigin = true;
5206 
5207  } else if (HP.IsKeyWord("origin" "position")) {
5208  Origin = HP.GetPosAbs(::AbsRefFrame);
5209  bOrigin = true;
5210  }
5211 
5212  if (bOrigin) {
5213  for (unsigned int iStrNode = 1; iStrNode <= NFEMNodes; iStrNode++) {
5214  pXYZFEMNodes->SubVec(iStrNode, Origin);
5215  }
5216 
5217  if (!bBuildInvariants) {
5218  XTmpIn -= Origin;
5219  JTmp = JTmpIn - Mat3x3(MatCrossCross, XTmpIn, XTmpIn*dMass);
5220  }
5221  }
5222 
5223  /* numero di nodi d'interfaccia */
5224  unsigned int NStrNodes = HP.GetInt();
5225  DEBUGCOUT("Number of Interface Nodes : " << NStrNodes << std::endl);
5226 
5227  SAFENEWWITHCONSTRUCTOR(pPHItStrNode, Mat3xN,
5228  Mat3xN(NStrNodes*NModes, 0.));
5229  SAFENEWWITHCONSTRUCTOR(pPHIrStrNode, Mat3xN,
5230  Mat3xN(NStrNodes*NModes, 0.));
5231 
5232  std::vector<Modal::StrNodeData> SND(NStrNodes);
5233 
5234  bool bOut = false;
5235  if (HP.IsKeyWord("output")) {
5236  bOut = HP.GetYesNoOrBool();
5237  }
5238 
5239  for (unsigned int iStrNode = 1; iStrNode <= NStrNodes; iStrNode++) {
5240  /* nodo collegato 1 (e' il nodo FEM) */
5241  std::string Node1 = HP.GetString("");
5242  // NOTE: we should check whether a label includes
5243  // whitespace. In any case, it wouldn't match
5244  // any of the labels read, so it is pointless,
5245  // but could help debugging...
5246  if (Node1.find(' ') != std::string::npos ) {
5247  silent_cout("Modal(" << uLabel << "): "
5248  "FEM node \"" << Node1 << "\""
5249  << " at line " << HP.GetLineData()
5250  << " contains a blank" << std::endl);
5251  }
5252 
5253  DEBUGCOUT("Linked to FEM Node " << Node1 << std::endl);
5254 
5255  unsigned int iNode;
5256  /* verifica di esistenza del nodo 1*/
5257  for (iNode = 0; iNode < NFEMNodes; iNode++) {
5258  if (Node1 == IdFEMNodes[iNode]) {
5259  break;
5260  }
5261  }
5262 
5263  if (iNode == NFEMNodes) {
5264  silent_cerr("Modal(" << uLabel << "): "
5265  "FEM node \"" << Node1 << "\""
5266  << " at line " << HP.GetLineData()
5267  << " not defined " << std::endl);
5268  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
5269  }
5270 
5271  iNode++;
5272 
5273  int iNodeCurr = iNode;
5274 
5275  /* recupera la posizione del nodo FEM, somma di posizione
5276  * e eventuale offset;
5277  *
5278  * HEADS UP: non piu' offset per i nodi FEM !!!!!!!!!
5279  *
5280  * nota: iNodeCurr contiene la posizione a cui si trova
5281  * il nodo FEM d'interfaccia nell'array pXYZNodes */
5282  SND[iStrNode-1].OffsetFEM = pXYZFEMNodes->GetVec(iNodeCurr);
5283 
5284  /* salva le forme modali del nodo d'interfaccia
5285  * nell'array pPHIStrNode */
5286  for (unsigned int jMode = 0; jMode < NModes; jMode++) {
5287  pPHItStrNode->PutVec(jMode*NStrNodes + iStrNode,
5288  pModeShapest->GetVec(jMode*NFEMNodes + iNodeCurr));
5289  pPHIrStrNode->PutVec(jMode*NStrNodes + iStrNode,
5290  pModeShapesr->GetVec(jMode*NFEMNodes + iNodeCurr));
5291  }
5292 
5293  /* nodo collegato 2 (e' il nodo multibody) */
5294  unsigned int uNode2 = (unsigned int)HP.GetInt();
5295  DEBUGCOUT("Linked to Multi-Body Node " << uNode2 << std::endl);
5296 
5297  /* verifica di esistenza del nodo 2 */
5298  SND[iStrNode - 1].pNode = pDM->pFindNode<const StructNode, Node::STRUCTURAL>(uNode2);
5299  if (SND[iStrNode - 1].pNode == 0) {
5300  silent_cerr("Modal(" << uLabel << "): "
5301  "StructuralNode(" << uNode2 << ") "
5302  "at line " << HP.GetLineData()
5303  << " not defined" << std::endl);
5304  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
5305  }
5306 
5307  /* offset del nodo Multi-Body */
5308  ReferenceFrame RF = ReferenceFrame(SND[iStrNode - 1].pNode);
5309  Vec3 d2(HP.GetPosRel(RF));
5310  Mat3x3 R2(::Eye3);
5311  if (HP.IsKeyWord("hinge") || HP.IsKeyWord("orientation")) {
5312  R2 = HP.GetRotRel(RF);
5313  }
5314 
5315  SND[iStrNode - 1].OffsetMB = d2;
5316  SND[iStrNode - 1].RotMB = R2; // FIXME: not used (so far)
5317 
5318  DEBUGCOUT("Multibody Node reference frame d2: " << std::endl
5319  << d2 << std::endl);
5320 
5321  /* salva le label dei nodi vincolati nell'array IntNodes;
5322  * puo' servire per il restart? */
5323  SND[iStrNode - 1].FEMNode = Node1;
5324 
5325  if (HP.IsKeyWord("output")) {
5326  SND[iStrNode - 1].bOut = HP.GetYesNoOrBool();
5327  }
5328 
5329  const Vec3& xMB(SND[iStrNode - 1].pNode->GetXCurr());
5330  pedantic_cout("Interface node " << iStrNode << ":" << std::endl
5331  << " MB node " << uNode2 << " x={" << xMB << "}" << std::endl);
5332  Vec3 xFEMRel(pXYZFEMNodes->GetVec(iNodeCurr));
5333  Vec3 xFEM(X0 + R*xFEMRel);
5334  pedantic_cout(" FEM node \"" << Node1 << "\" x={" << xFEM << "} "
5335  "xrel={" << xFEMRel << "}" << std::endl);
5336  pedantic_cout(" offset={" << xFEM - xMB << "}" << std::endl);
5337  } /* fine ciclo sui nodi d'interfaccia */
5338 
5339  if (bOut) {
5340  std::vector<Modal::StrNodeData>::iterator i = SND.begin();
5341  std::vector<Modal::StrNodeData>::iterator end = SND.end();
5342  for (; i != end; ++i) {
5343  i->bOut = bOut;
5344  }
5345  }
5346 
5347  /* fine ciclo caricamento dati */
5348 
5349  /*
5350  * calcola gli invarianti d'inerzia (massa, momenti statici
5351  * e d'inerzia, termini di accoppiamento nel SdR locale)
5352  */
5353 
5354  /* NOTE: right now, either they are all used (except for Inv9)
5355  * or none is used, and the global inertia of the body is expected */
5356  if (bBuildInvariants) {
5357  doublereal dMassInv = 0.;
5358  Vec3 STmpInv(::Zero3);
5359  Mat3x3 JTmpInv(::Zero3x3);
5360 
5361  MatNxN GenMass(NModes, 0.);
5362 
5363  /* TODO: select what terms are used */
5364 
5365  SAFENEWWITHCONSTRUCTOR(pInv3, Mat3xN, Mat3xN(NModes, 0.));
5366  SAFENEWWITHCONSTRUCTOR(pInv4, Mat3xN, Mat3xN(NModes, 0.));
5367 
5368  /* NOTE: we assume that Inv5 is used only if Inv4 is used as well */
5369  /* Inv5 e' un 3xMxM */
5370  SAFENEWWITHCONSTRUCTOR(pInv5, Mat3xN, Mat3xN(NModes*NModes, 0.));
5371 
5372  /* Inv8 e' un 3x3xM */
5373  SAFENEWWITHCONSTRUCTOR(pInv8, Mat3xN, Mat3xN(3*NModes, 0.));
5374 
5375  /* NOTE: we assume that Inv9 is used only if Inv8 is used as well */
5376  /* by default: no */
5377  if (HP.IsKeyWord("use" "invariant" "9")) {
5378  /* Inv9 e' un 3x3xMxM */
5379  SAFENEWWITHCONSTRUCTOR(pInv9, Mat3xN, Mat3xN(3*NModes*NModes, 0.));
5380  }
5381  /* Inv10 e' un 3x3xM */
5382  SAFENEWWITHCONSTRUCTOR(pInv10,Mat3xN, Mat3xN(3*NModes, 0.));
5383  SAFENEWWITHCONSTRUCTOR(pInv11,Mat3xN, Mat3xN(NModes, 0.));
5384 
5385  /* inizio ciclo scansione nodi */
5386  for (unsigned int iNode = 1; iNode <= NFEMNodes; iNode++) {
5387  doublereal mi = FEMMass[iNode - 1];
5388 
5389  /* massa totale (Inv 1) */
5390  dMassInv += mi;
5391 
5392  /* posizione nodi FEM */
5393  Vec3 ui = pXYZFEMNodes->GetVec(iNode);
5394 
5395  Mat3x3 uiWedge(MatCross, ui);
5396  Mat3x3 JiNodeTmp(::Zero3x3);
5397 
5398  JiNodeTmp(1, 1) = FEMJ[iNode - 1](1);
5399  JiNodeTmp(2, 2) = FEMJ[iNode - 1](2);
5400  JiNodeTmp(3, 3) = FEMJ[iNode - 1](3);
5401 
5402  JTmpInv += JiNodeTmp - Mat3x3(MatCrossCross, ui, ui*mi);
5403  STmpInv += ui*mi;
5404 
5405  /* estrae le forme modali del nodo i-esimo */
5406  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
5407  unsigned int iOffset = (jMode - 1)*NFEMNodes + iNode;
5408 
5409  PHIti.PutVec(jMode, pModeShapest->GetVec(iOffset));
5410  PHIri.PutVec(jMode, pModeShapesr->GetVec(iOffset));
5411  }
5412 
5413  /* TODO: only build what is required */
5414 
5415  Mat3xN Inv3Tmp(NModes, 0.);
5416  Mat3xN Inv4Tmp(NModes, 0.);
5417  Mat3xN Inv4JTmp(NModes, 0.);
5418  Inv3Tmp.Copy(PHIti);
5419 
5420  /* Inv3 = mi*PHIti, i = 1,...nnodi */
5421  Inv3Tmp *= mi;
5422 
5423  /* Inv4 = mi*ui/\*PHIti + Ji*PHIri, i = 1,...nnodi */
5424  Inv4Tmp.LeftMult(uiWedge*mi, PHIti);
5425  Inv4JTmp.LeftMult(JiNodeTmp, PHIri);
5426  Inv4Tmp += Inv4JTmp;
5427  *pInv3 += Inv3Tmp;
5428  *pInv4 += Inv4Tmp;
5429  *pInv11 += Inv4JTmp;
5430 
5431  /* inizio ciclo scansione modi */
5432  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
5433  Vec3 PHItij = PHIti.GetVec(jMode);
5434  Vec3 PHIrij = PHIri.GetVec(jMode);
5435 
5436  Mat3x3 PHItijvett_mi(MatCross, PHItij*mi);
5437  Mat3xN Inv5jTmp(NModes, 0);
5438 
5439  /* Inv5 = mi*PHItij/\*PHIti,
5440  * i = 1,...nnodi, j = 1,...nmodi */
5441  Inv5jTmp.LeftMult(PHItijvett_mi, PHIti);
5442  for (unsigned int kMode = 1; kMode <= NModes; kMode++) {
5443  pInv5->AddVec((jMode - 1)*NModes + kMode,
5444  Inv5jTmp.GetVec(kMode));
5445 
5446  /* compute the modal mass matrix
5447  * using the FEM inertia and the
5448  * mode shapes */
5449  GenMass(jMode, kMode) += (PHItij*PHIti.GetVec(kMode))*mi
5450  + PHIrij*(JiNodeTmp*PHIri.GetVec(kMode));
5451  }
5452 
5453  /* Inv8 = -mi*ui/\*PHItij/\,
5454  * i = 1,...nnodi, j = 1,...nmodi */
5455  Mat3x3 Inv8jTmp = -uiWedge*PHItijvett_mi;
5456  pInv8->AddMat3x3((jMode - 1)*3 + 1, Inv8jTmp);
5457 
5458  /* Inv9 = mi*PHItij/\*PHItik/\,
5459  * i = 1,...nnodi, j, k = 1...nmodi */
5460  if (pInv9 != 0) {
5461  for (unsigned int kMode = 1; kMode <= NModes; kMode++) {
5462  Mat3x3 PHItikvett(MatCross, PHIti.GetVec(kMode));
5463  Mat3x3 Inv9jkTmp = PHItijvett_mi*PHItikvett;
5464 
5465  pInv9->AddMat3x3((jMode - 1)*3*NModes + (kMode - 1)*3 + 1, Inv9jkTmp);
5466  }
5467  }
5468 
5469  /* Inv10 = [PHIrij/\][J0i],
5470  * i = 1,...nnodi, j = 1,...nmodi */
5471  Mat3x3 Inv10jTmp = PHIrij.Cross(JiNodeTmp);
5472  pInv10->AddMat3x3((jMode - 1)*3 + 1, Inv10jTmp);
5473  } /* fine ciclo scansione modi */
5474  } /* fine ciclo scansione nodi */
5475 
5476  if (bRecordGroup[12]) {
5477  Mat3x3 DJ = JTmp - JTmpInv;
5478  pedantic_cerr(" Rigid-body mass: "
5479  "input - computed" << std::endl
5480  << " " << dMass - dMassInv << std::endl);
5481  pedantic_cerr(" Rigid-body CM location: "
5482  "input - computed" << std::endl
5483  << " " << XTmpIn - STmpInv/dMassInv << std::endl);
5484  pedantic_cerr(" Rigid-body inertia: "
5485  "input - computed" << std::endl
5486  << " " << DJ.GetVec(1) << std::endl
5487  << " " << DJ.GetVec(2) << std::endl
5488  << " " << DJ.GetVec(3) << std::endl);
5489  }
5490 
5491  /*
5492  * TODO: check modal mass
5493  */
5494  pedantic_cerr(" Generalized mass: input - computed" << std:: endl);
5495  for (unsigned int jMode = 1; jMode <= NModes; jMode++) {
5496  pedantic_cerr(" ");
5497  for (unsigned int kMode = 1; kMode <= NModes; kMode++) {
5498  pedantic_cerr(" " << pGenMass->dGet(jMode, kMode) - GenMass(jMode, kMode));
5499  }
5500  pedantic_cerr(std::endl);
5501  }
5502 
5503  // if record 12 was read, leave its data in place,
5504  // otherwise replace rigid-body invariants with those from record 12
5505  if (!bRecordGroup[12]) {
5506  dMass = dMassInv;
5507  STmp = STmpInv;
5508  JTmp = JTmpInv;
5509  }
5510  }
5511 
5512  // if record 12 was read, fix static moment
5513  if (bRecordGroup[12]) {
5514  /* left over when reading XCG */
5515  STmp = XTmpIn*dMass;
5516  }
5517 
5518  /*
5519  * TODO: Check rank of modal stiffness matrix
5520  */
5521 
5522  // check if mass matrix is symmetric and diagonal
5523  bool bIsMSym = true;
5524  bool bIsMDiag = true;
5525  for (unsigned iRow = 2; iRow <= NModes; iRow++) {
5526  for (unsigned iCol = 1; iCol < iRow; iCol++) {
5527  doublereal mrc = pGenMass->dGet(iRow, iCol);
5528  doublereal mcr = pGenMass->dGet(iCol, iRow);
5529  if (mrc != mcr) {
5530  if (bIsMSym) {
5531  silent_cerr("Modal(" << uLabel << "): mass matrix is not symmetric: (at least) "
5532  "M(" << iRow << ", " << iCol << ")=" << mrc << " "
5533  "!= "
5534  "M(" << iCol << ", " << iRow << ")=" << mcr << " "
5535  << std::endl);
5536  }
5537  bIsMSym = false;
5538  }
5539 
5540  if (mrc != 0. || mcr != 0.) {
5541  if (bIsMDiag) {
5542  silent_cerr("Modal(" << uLabel << "): mass matrix is not diagonal: (at least) "
5543  "M(" << iRow << ", " << iCol << ")=" << mrc << " "
5544  "and/or "
5545  "M(" << iCol << ", " << iRow << ")=" << mcr << " "
5546  "!= 0.0"
5547  << std::endl);
5548  }
5549  bIsMDiag = false;
5550  }
5551  }
5552  }
5553 
5554  bool bIsKSym = true;
5555  bool bIsKDiag = true;
5556  for (unsigned iRow = 2; iRow <= NModes; iRow++) {
5557  for (unsigned iCol = 1; iCol < iRow; iCol++) {
5558  doublereal mrc = pGenStiff->dGet(iRow, iCol);
5559  doublereal mcr = pGenStiff->dGet(iCol, iRow);
5560  if (mrc != mcr) {
5561  if (bIsKSym) {
5562  silent_cerr("Modal(" << uLabel << "): stiffness matrix is not symmetric: (at least) "
5563  "K(" << iRow << ", " << iCol << ")=" << mrc << " "
5564  "!= "
5565  "K(" << iCol <<