MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
inertia.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/inertia.cc,v 1.29 2017/06/18 23:06:24 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 /* inertia element */
33 
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35 
36 #include <cfloat>
37 #include <set>
38 #include <limits>
39 
40 #include "inertia.h"
41 #include "dataman.h"
42 #include "Rot.hh"
43 
44 /* CenterOfMass - begin */
45 
46 std::ostream&
47 CenterOfMass::Output_int(std::ostream& out) const
48 {
49  return out
50  << " mass: " << dMass << std::endl
51  << " J: " << J << std::endl
52  << " Xcg: " << X_cm << std::endl
53  << " Jcg: " << J_cm << std::endl
54  << " Vcg: " << V_cm << std::endl
55  << " Wcg: " << Omega_cm << std::endl;
56 }
57 
58 void
60 {
61  dMass = 0.;
62  S = Zero3;
63  J = Zero3x3;
64 
65  Vec3 B(Zero3), G(Zero3);
66 
67  for (std::set<const ElemGravityOwner *>::const_iterator i = elements.begin();
68  i != elements.end(); ++i)
69  {
70  dMass += (*i)->dGetM();
71  S += (*i)->GetS();
72  J += (*i)->GetJ();
73 
74  B += (*i)->GetB();
75  G += (*i)->GetG();
76  }
77 
78  J_cm = J;
79  if (dMass < std::numeric_limits<doublereal>::epsilon()) {
80  X_cm = Zero3;
81  V_cm = Zero3;
82  Omega_cm = Zero3;
83  J_cm = J_cm.Symm();
84 
85  } else {
86  X_cm = S/dMass;
87  V_cm = B/dMass;
88 
89  /*
90  * FIXME: should also rotate it in the principal
91  * reference frame, and log the angles
92  */
94  J_cm = J_cm.Symm();
95 
96  ASSERT(J_cm.IsSymmetric()); // NOTE: should be a run time test
97  Omega_cm = J_cm.LDLSolve(G - X_cm.Cross(B));
98  }
99 }
100 
101 /* Costruttore definitivo (da mettere a punto) */
102 CenterOfMass::CenterOfMass(std::set<const ElemGravityOwner *>& elements) :
103 elements(elements)
104 {
105  NO_OP;
106 }
107 
109 {
110  NO_OP;
111 }
112 
113 /* CenterOfMass - end */
114 
115 /* Inertia - begin */
116 
117  /* momento statico */
118 Vec3
119 Inertia::GetS_int(void) const
120 {
121  return S;
122 }
123 
124 /* momento d'inerzia */
125 Mat3x3
126 Inertia::GetJ_int(void) const
127 {
128  return J;
129 }
130 
131 std::ostream&
132 Inertia::Output_int(std::ostream& out) const
133 {
134  out
135  << "inertia: " << GetLabel()
136  << ( GetName().empty() ? "" : ( std::string(" \"") + GetName() + "\"" ) )
137  << std::endl;
138  Vec3 DX(X_cm - X0);
139  Mat3x3 JX(J_cm - Mat3x3(MatCrossCross, DX, DX*dMass));
141  << " Xcg-X: " << DX << std::endl
142  << " R^T*(Xcg-X): " << R0.MulTV(DX) << std::endl
143  << " J(X): " << JX << std::endl
144  << " R^T*J(X)*R: " << R0.MulTM(JX)*R0 << std::endl
145  << " Rp: " << R_princ << std::endl
146  << " Thetap: " << RotManip::VecRot(R_princ) << std::endl
147  << " Jp: " << J_princ << std::endl;
148 
149  //printf("\n\nmassa %1.16e\n",dMass);
150  //Vec3 tmp = R0.MulTV(X_cm-X0);
151  //printf("xcg %1.16e %1.16e %1.16e\n", tmp(1), tmp(2), tmp(3) );
152  //printf("RR %1.16e %1.16e %1.16e\n", R_princ(1,1), R_princ(1,2), R_princ(1,3) );
153  //printf("RR %1.16e %1.16e %1.16e\n", R_princ(2,1), R_princ(2,2), R_princ(2,3) );
154  //printf("RR %1.16e %1.16e %1.16e\n", R_princ(3,1), R_princ(3,2), R_princ(3,3) );
155  //printf("JJ %1.16e %1.16e %1.16e\n\n\n", J_princ(1), J_princ(2), J_princ(3) );
156  return out;
157 }
158 
159 /* Costruttore definitivo (da mettere a punto) */
160 Inertia::Inertia(unsigned int uL, const std::string& sN, std::set<const ElemGravityOwner *>& elements,
161  const Vec3& x0, const Mat3x3& r0, std::ostream& log, flag fOut)
162 : Elem(uL, fOut),
163 ElemGravityOwner(uL, fOut),
164 InitialAssemblyElem(uL, fOut),
165 CenterOfMass(elements),
166 #ifdef USE_NETCDF
167 Var_dMass(0),
168 Var_X_cm(0),
169 Var_V_cm(0),
170 Var_Omega_cm(0),
171 Var_DX(0),
172 Var_dx(0),
173 Var_Jp(0),
174 Var_Phip(0),
175 #endif // USE_NETCDF
176 flags(0), X0(x0), R0(r0)
177 {
178  this->PutName(sN);
179 
180  if (!X0.IsNull()) {
181  flags |= 1;
182  }
183 
184  if (!(R0 - Eye3).IsNull()) {
185  flags |= 2;
186  }
187 
188  bool done(false);
189  if ((fOut & 0x1) & !silent_output) {
190  if (!done) {
191  Collect_int();
192  done = true;
193  }
194  Output_int(std::cout);
195  }
196 
197  if (fOut & 0x2) {
198  if (!done) {
199  Collect_int();
200  done = true;
201  }
202  Output_int(log);
203  }
204 }
205 
207 {
208  NO_OP;
209 }
210 
211 /* massa totale */
213 Inertia::dGetM(void) const
214 {
215  return dMass;
216 }
217 
218 /* Tipo dell'elemento (usato solo per debug ecc.) */
221 {
222  return Elem::INERTIA;
223 }
224 
225 /* Numero gdl durante l'assemblaggio iniziale */
226 unsigned int
228 {
229  return 0;
230 }
231 
232 void
234 {
236 
237  if (dMass < std::numeric_limits<doublereal>::epsilon()) {
238  silent_cerr("Inertia(" << GetLabel() << "): "
239  "mass is null" << std::endl);
240 
241  R_princ = Eye3;
242  J_princ = Zero3;
243 
244  } else {
246  }
247 
248  if (flags) {
249  if (flags & 1) {
250  Vec3 DX = X_cm - X0;
251  J0 = J + Mat3x3(MatCrossCross, X0, X0*dMass)
252  + Mat3x3(MatCrossCross, DX, X0*dMass)
253  + Mat3x3(MatCrossCross, X0, DX*dMass);
254  }
255 
256  if (flags & 2) {
257  if (flags & 1) {
258  J0 = R0*J0.MulMT(R0);
259  } else {
260  J0 = R0*J.MulMT(R0);
261  }
262  }
263 
264  } else {
265  J0 = J;
266  }
267 }
268 
269 /* Scrive il contributo dell'elemento al file di restart */
270 std::ostream&
271 Inertia::Restart(std::ostream& out) const
272 {
273  return out;
274 }
275 
276 void
278 {
279  if (fToBeOutput() & (0x1 | Inertia::OUTPUT_ALWAYS)) {
281  Output_int(std::cout);
282  }
283 
284 #ifdef USE_NETCDF
286 
287  Vec3 DX(X_cm - X0);
288  Vec3 dx = R0.MulTV(DX);
289  Vec3 Phip = RotManip::VecRot(R_princ);
290 
291  Var_dMass->put_rec(&dMass, OH.GetCurrentStep());
292  Var_X_cm->put_rec(X_cm.pGetVec(), OH.GetCurrentStep());
293  Var_V_cm->put_rec(V_cm.pGetVec(), OH.GetCurrentStep());
294  Var_Omega_cm->put_rec(Omega_cm.pGetVec(), OH.GetCurrentStep());
295 
296  Var_DX->put_rec(DX.pGetVec(), OH.GetCurrentStep());
297  Var_dx->put_rec(dx.pGetVec(), OH.GetCurrentStep());
298  Var_Jp->put_rec(J_princ.pGetVec(), OH.GetCurrentStep());
299  Var_Phip->put_rec(Phip.pGetVec(), OH.GetCurrentStep());
300 
301  }
302 #endif // USE_NETCDF
303  }
304 }
305 
306 void
308 {
309 #ifdef USE_NETCDF
311 
312  std::ostringstream os;
313  os << "elem.inertia." << GetLabel();
314  (void)OH.CreateVar(os.str(), "inertia");
315 
316  os << ".";
317  name = os.str();
318 
319 #endif // USE_NETCDF
320 }
321 
322 void
324 {
325  if (bToBeOutput()) {
326 #ifdef USE_NETCDF
328  {
329  std::string name;
330  OutputPrepare_int(OH, name);
331 
332  Var_dMass = OH.CreateVar<doublereal>(name + "M", "kg",
333  "total mass");
334  Var_X_cm = OH.CreateVar<Vec3>(name + "X_cm", "m",
335  "center of mass position (x, y, z)");
336  Var_V_cm = OH.CreateVar<Vec3>(name + "V_cm", "m/s",
337  "center of mass velocity (x, y, z)");
338  Var_Omega_cm = OH.CreateVar<Vec3>(name + "Omega_cm", "rad/s",
339  "center of mass angular velocity (x, y, z)");
340 
341  Var_DX = OH.CreateVar<Vec3>(name + "DX", "m",
342  "relative center of mass position, global frame (x, y, z)");
343  Var_dx = OH.CreateVar<Vec3>(name + "dx", "m",
344  "relative center of mass position, local frame (x, y, z)");
345  Var_Jp = OH.CreateVar<Vec3>(name + "Jp", "kg*m^2",
346  "global inertia matrix, w.r.t. principal axes");
347  Var_Phip = OH.CreateVar<Vec3>(name + "Phip", "-",
348  "orientation vector of principal axes, global frame");
349 
350  }
351 #endif // USE_NETCDF
352  }
353 }
354 
355 void
356 Inertia::WorkSpaceDim(integer* piNumRows, integer* piNumCols) const
357 {
358  *piNumRows = 0;
359  *piNumCols = 0;
360 }
361 
364  doublereal dCoef,
365  const VectorHandler& XCurr,
366  const VectorHandler& XPrimeCurr)
367 {
368  WorkMat.SetNullMatrix();
369  return WorkMat;
370 }
371 
374  doublereal dCoef,
375  const VectorHandler& XCurr,
376  const VectorHandler& XPrimeCurr)
377 {
378  WorkVec.Resize(0);
379 
380  Collect_int();
381 
382  return WorkVec;
383 }
384 
385 bool
387 {
388  return true;
389 }
390 
391 /* Inverse Dynamics residual assembly */
394  const VectorHandler& XCurr,
395  const VectorHandler& XPrimeCurr,
396  const VectorHandler& /* XPrimePrimeCurr */,
397  InverseDynamics::Order iOrder)
398 {
399  WorkVec.Resize(0);
400 
401  Collect_int();
402 
403  return WorkVec;
404 }
405 
406 /* inverse dynamics Jacobian matrix assembly */
409  const VectorHandler& /* XCurr */)
410 {
411  WorkMat.SetNullMatrix();
412  return WorkMat;
413 }
414 
415 /* Dimensione del workspace durante l'assemblaggio iniziale.
416  * Occorre tener conto del numero di dof che l'elemento definisce
417  * in questa fase e dei dof dei nodi che vengono utilizzati.
418  * Sono considerati dof indipendenti la posizione e la velocita'
419  * dei nodi */
420 void
421 Inertia::InitialWorkSpaceDim(integer* piNumRows, integer* piNumCols) const
422 {
423  *piNumRows = 0;
424  *piNumCols = 0;
425 }
426 
427 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
430  const VectorHandler& XCurr)
431 {
432  WorkMat.SetNullMatrix();
433  return WorkMat;
434 }
435 
436 /* Contributo al residuo durante l'assemblaggio iniziale */
439 {
440  WorkVec.Resize(0);
441 
442  Collect_int();
443 
444  return WorkVec;
445 }
446 
447 /* Usata per inizializzare la quantita' di moto */
448 void
452 {
453  NO_OP;
454 }
455 
456 unsigned int
458 {
459  return
460  +3 // X[1-3]
461  +3 // Phi[1-3]
462  +3 // V[1-3]
463  +3 // Omega[1-3]
464  +3 // principal inertia moments
465  +3*3 // inertia tensor
466  +1 // mass
467  ;
468 }
469 
470 unsigned int
471 Inertia::iGetPrivDataIdx(const char *s) const
472 {
473  unsigned int idx = 0;
474 
475  switch (s[0]) {
476  case 'X':
477  break;
478 
479  case 'P':
480  if (strncasecmp(s, "Phi", STRLENOF("Phi")) != 0) {
481  return 0;
482  }
483  s += STRLENOF("Phi") - 1;
484  idx = 3;
485  break;
486 
487  case 'V':
488  idx = 6;
489  break;
490 
491  case 'O':
492  if (strncasecmp(s, "Omega", STRLENOF("Omega")) != 0) {
493  return 0;
494  }
495  s += STRLENOF("Omega") - 1;
496  idx = 9;
497  break;
498 
499  case 'J':
500  if (s[1] == 'P') {
501  s++;
502  idx = 12;
503  break;
504  }
505  idx = 15;
506  break;
507 
508  case 'm':
509  if (s[1] != '\0') {
510  return 0;
511  }
512  return 25;
513 
514  default:
515  return 0;
516  }
517 
518  s++;
519  if (s[0] != '[') {
520  return 0;
521  }
522 
523  s++;
524  switch (s[0]) {
525  case '1':
526  case '2':
527  case '3':
528  idx += s[0] - '0';
529  s++;
530  if (idx > 15) {
531  if (s[0] != ',') {
532  return 0;
533  }
534  s++;
535  switch (s[0]) {
536  case '1':
537  case '2':
538  case '3':
539  break;
540 
541  default:
542  return 0;
543  }
544  idx += 3*(s[0] - '1');
545  s++;
546  }
547  break;
548 
549  default:
550  return 0;
551  }
552 
553  //s++;
554  if (strcmp(s, "]") != 0) {
555  return 0;
556  }
557  return idx;
558 }
559 
561 Inertia::dGetPrivData(unsigned int i) const
562 {
563 
564  unsigned int what = (i - 1)/3;
565  unsigned int which = (i - 1)%3 + 1;
566 
567  switch (what) {
568  case 0:
569  // center of mass position
570  return X_cm(which);
571 
572  case 1:
573  // principal inertia axes Euler vector components
574  return 0.;
575 
576  case 2:
577  // center of mass velocity
578  return V_cm(which);
579 
580  case 3:
581  // angular velocity
582  return Omega_cm(which);
583 
584  case 4:
585  // principal inertia moments
586  return J_princ(which);
587  }
588 
589  // mass
590  if (i == 25) {
591  return dMass;
592  }
593 
594  // inertia tensor
595  int ir = (i - 15 - 1)%3 + 1;
596  int ic = (i - 15 - 1)/3 + 1;
597 
598  return J0(ir, ic);
599 
601 }
void PutName(const std::string &sN)
Definition: withlab.cc:56
virtual SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: inertia.cc:438
virtual void Output(OutputHandler &OH) const
Definition: inertia.cc:277
const Vec3 Zero3(0., 0., 0.)
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
long int flag
Definition: mbdyn.h:43
virtual bool bToBeOutput(void) const
Definition: output.cc:890
virtual SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: inertia.cc:373
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
Definition: matvec3.h:98
bool UseNetCDF(int out) const
Definition: output.cc:491
Mat3x3 R_princ
Definition: inertia.h:87
void OutputPrepare(OutputHandler &OH)
Definition: inertia.cc:323
Vec3 V_cm
Definition: inertia.h:53
const Mat3x3 Eye3(1., 0., 0., 0., 1., 0., 0., 0., 1.)
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: inertia.cc:421
virtual bool bInverseDynamics(void) const
Definition: inertia.cc:386
std::set< const ElemGravityOwner * > elements
Definition: inertia.h:45
#define NO_OP
Definition: myassert.h:74
virtual std::ostream & Output_int(std::ostream &out) const
Definition: inertia.cc:132
std::vector< Hint * > Hints
Definition: simentity.h:89
Mat3x3 J_cm
Definition: inertia.h:55
virtual unsigned int iGetInitialNumDof(void) const
Definition: inertia.cc:227
Mat3x3 J0
Definition: inertia.h:85
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
virtual VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: inertia.cc:429
Vec3 MulTV(const Vec3 &v) const
Definition: matvec3.cc:482
virtual unsigned int iGetPrivDataIdx(const char *s) const
Definition: inertia.cc:471
doublereal dMass
Definition: inertia.h:47
const Mat3x3 Zero3x3(0., 0., 0., 0., 0., 0., 0., 0., 0.)
virtual unsigned int iGetNumPrivData(void) const
Definition: inertia.cc:457
Vec3 Omega_cm
Definition: inertia.h:54
void OutputPrepare_int(OutputHandler &OH, std::string &name)
Definition: inertia.cc:307
bool IsNull(void) const
Definition: matvec3.h:472
GradientExpression< UnaryExpr< FuncLog, Expr > > log(const GradientExpression< Expr > &u)
Definition: gradient.h:2976
void SetNullMatrix(void)
Definition: submat.h:1159
long GetCurrentStep(void) const
Definition: output.h:116
virtual VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: inertia.cc:363
doublereal dGetM(void) const
Definition: inertia.cc:213
CenterOfMass(std::set< const ElemGravityOwner * > &elements)
Definition: inertia.cc:102
virtual doublereal dGetPrivData(unsigned int i) const
Definition: inertia.cc:561
unsigned flags
Definition: inertia.h:82
bool EigSym(Vec3 &EigenValues) const
Definition: matvec3.cc:255
bool IsOpen(int out) const
Definition: output.cc:395
#define ASSERT(expression)
Definition: colamd.c:977
void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: inertia.cc:356
Vec3 X_cm
Definition: inertia.h:52
Mat3x3 MulTM(const Mat3x3 &m) const
Definition: matvec3.cc:500
virtual ~CenterOfMass(void)
Definition: inertia.cc:108
virtual void Collect_int(void)
Definition: inertia.cc:233
Vec3 LDLSolve(const Vec3 &v) const
Definition: matvec3.cc:199
bool IsSymmetric(void) const
Definition: matvec3.h:1260
Vec3 J_princ
Definition: inertia.h:88
Vec3 X0
Definition: inertia.h:83
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
#define STRLENOF(s)
Definition: mbdyn.h:166
Mat3x3 Symm(void) const
Definition: matvec3.h:840
virtual Elem::Type GetElemType(void) const
Definition: inertia.cc:220
Mat3x3 R0
Definition: inertia.h:84
Definition: elem.h:75
virtual std::ostream & Restart(std::ostream &out) const
Definition: inertia.cc:271
Type
Definition: elem.h:91
Mat3x3 J
Definition: inertia.h:49
static unsigned done
Definition: gust.cc:209
const doublereal * pGetVec(void) const
Definition: matvec3.h:192
virtual ~Inertia(void)
Definition: inertia.cc:206
virtual flag fToBeOutput(void) const
Definition: output.cc:884
Mat3x3 MulMT(const Mat3x3 &m) const
Definition: matvec3.cc:444
Mat3x3 GetJ_int(void) const
Definition: inertia.cc:126
Inertia(unsigned int uL, const std::string &sN, std::set< const ElemGravityOwner * > &elements, const Vec3 &x0, const Mat3x3 &r0, std::ostream &log, flag fOut)
Definition: inertia.cc:160
Vec3 S
Definition: inertia.h:48
const std::string & GetName(void) const
Definition: withlab.cc:68
double doublereal
Definition: colamd.c:52
long int integer
Definition: colamd.c:51
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: inertia.cc:449
unsigned int GetLabel(void) const
Definition: withlab.cc:62
virtual void Collect_int(void)
Definition: inertia.cc:59
Vec3 GetS_int(void) const
Definition: inertia.cc:119
virtual std::ostream & Output_int(std::ostream &out) const
Definition: inertia.cc:47
virtual void Resize(integer iNewSize)=0
bool UseText(int out) const
Definition: output.cc:446