1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/elec/accelerometer.cc,v 1.32 2017/01/12 14:46:22 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 /* Elementi elettrici */
33
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35
36 #include "strnode.h"
37 #include "elecnode.h"
38 #include "accelerometer.h"
39
40 /* Accelerometer - begin */
41
42 /* Costruttore */
Accelerometer(unsigned int uL,const DofOwner * pDO,const StructNode * pS,const ScalarDifferentialNode * pA,const Vec3 & TmpDir,doublereal dO,doublereal dT,doublereal dC,doublereal dK,flag fOut)43 Accelerometer::Accelerometer(unsigned int uL, const DofOwner* pDO,
44 const StructNode* pS,
45 const ScalarDifferentialNode* pA,
46 const Vec3& TmpDir,
47 doublereal dO, doublereal dT,
48 doublereal dC, doublereal dK,
49 flag fOut)
50 : Elem(uL, fOut),
51 Electric(uL, pDO, fOut),
52 pStrNode(pS), pAbsNode(pA),
53 Dir(TmpDir), dOmega(dO), dTau(dT), dCsi(dC), dKappa(dK)
54 {
55 NO_OP;
56 }
57
58 /* Distruttore banale */
~Accelerometer(void)59 Accelerometer::~Accelerometer(void)
60 {
61 NO_OP;
62 }
63
64 /* Contributo al file di restart */
65 std::ostream&
Restart(std::ostream & out) const66 Accelerometer::Restart(std::ostream& out) const
67 {
68 Electric::Restart(out) << ", accelerometer, "
69 << pStrNode->GetLabel() << ", "
70 << pAbsNode->GetLabel() << ", "
71 "reference, node, ", Dir.Write(out, ", ") << ", "
72 "node, "
73 << dOmega << ", "
74 << dTau << ", "
75 << dCsi << ", "
76 << dKappa << ';'
77 << std::endl;
78 return out;
79 }
80
81 /* Costruisce il contributo allo jacobiano */
82 VariableSubMatrixHandler&
AssJac(VariableSubMatrixHandler & WorkMat,doublereal dCoef,const VectorHandler &,const VectorHandler &)83 Accelerometer::AssJac(VariableSubMatrixHandler& WorkMat,
84 doublereal dCoef,
85 const VectorHandler& /* XCurr */ ,
86 const VectorHandler& /* XPrimeCurr */ )
87 {
88 DEBUGCOUT("Entering Accelerometer::AssJac()" << std::endl);
89
90 /* Casting di WorkMat */
91 SparseSubMatrixHandler& WM = WorkMat.SetSparse();
92 WM.ResizeReset(15, 0);
93
94 /* Indici delle equazioni */
95 integer iFirstPositionIndex = pStrNode->iGetFirstPositionIndex();
96 integer iAbstractIndex = pAbsNode->iGetFirstIndex();
97 integer iFirstIndex = iGetFirstIndex();
98
99 /*
100 * | c 0 0 0 -a^T c*xP^T*a/\ || Delta_vP |
101 * | 0 1 c*O^2/T 0 0 0 || Delta_y1P |
102 * | 0 0 1+c*(2*C*O+1/T) -c 0 0 || Delta_y2P |
103 * |-K*O^2 -c c*O*(1+2*C/T) 1 0 0 || Delta_zP | = res
104 * | Delta_xP |
105 * | Delta_gP |
106 *
107 * con: c = dCoef
108 * a = RNode*Dir
109 * xp = Velocita' del nodo
110 * O = dOmega
111 * T = dTau
112 * C = dCsi
113 * K = dKappa
114 *
115 * v e' la misura della veocita' del punto,
116 * y1 e y2 sono stati dell'acceleromtro; v, y1, y2 appartengono al DofOwner
117 * dell'elemento;
118 * z e' la variabile del nodo astratto;
119 * x e g sono posizione e parametri di rotazione del nodo strutturale
120 *
121 *
122 * Funzione di trasferimento dell'accelerometro nel dominio di Laplace:
123 *
124 * e0 T * s
125 * ----(s) = K * ---------------------------------
126 * acc. (1 + T*s)*(1 + 2*C/O*s + s^2/O^2)
127 *
128 */
129
130 /* Dinamica dell'accelerometro */
131 WM.PutItem(1, iFirstIndex + 1, iFirstIndex + 1, dCoef);
132 WM.PutItem(2, iAbstractIndex + 1, iFirstIndex + 1,
133 -dKappa*dOmega*dOmega);
134 WM.PutItem(3, iFirstIndex + 2, iFirstIndex + 2, 1.);
135 WM.PutItem(4, iAbstractIndex + 1, iFirstIndex + 2, -dCoef);
136 WM.PutItem(5, iFirstIndex + 2, iFirstIndex + 3,
137 dCoef*dOmega*dOmega/dTau);
138 WM.PutItem(6, iFirstIndex + 3, iFirstIndex + 3,
139 1. + dCoef*(2.*dCsi*dOmega + 1./dTau));
140 WM.PutItem(7, iAbstractIndex + 1, iFirstIndex + 3,
141 dCoef*dOmega*(dOmega + 2.*dCsi/dTau));
142 WM.PutItem(8, iFirstIndex + 3, iAbstractIndex + 1, -dCoef);
143 WM.PutItem(9, iAbstractIndex + 1, iAbstractIndex + 1, 1.);
144
145 /* Misura dell'accelerazione */
146 Vec3 TmpDir((pStrNode->GetRRef())*Dir);
147 for (int iCnt = 1; iCnt <= 3; iCnt++) {
148 WM.PutItem(9 + iCnt, iFirstIndex + 1,
149 iFirstPositionIndex + iCnt,
150 - TmpDir.dGet(iCnt));
151 }
152
153 Vec3 XP(pStrNode->GetVCurr());
154 TmpDir = -TmpDir.Cross(XP);
155 for (int iCnt = 1; iCnt <= 3; iCnt++) {
156 WM.PutItem(12 + iCnt, iFirstIndex + 1,
157 iFirstPositionIndex + 3 + iCnt,
158 dCoef*TmpDir.dGet(iCnt));
159 }
160
161 return WorkMat;
162 }
163
164 /* Costruisce il contributo al residuo */
165 SubVectorHandler&
AssRes(SubVectorHandler & WorkVec,doublereal,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)166 Accelerometer::AssRes(SubVectorHandler& WorkVec,
167 doublereal /* dCoef */ ,
168 const VectorHandler& XCurr,
169 const VectorHandler& XPrimeCurr)
170 {
171 DEBUGCOUT("Entering Accelerometer::AssRes()" << std::endl);
172
173 /* Dimensiona e resetta la matrice di lavoro */
174 integer iNumRows = 0;
175 integer iNumCols = 0;
176 WorkSpaceDim(&iNumRows, &iNumCols);
177 WorkVec.ResizeReset(iNumRows);
178
179 integer iAbstractIndex = pAbsNode->iGetFirstIndex();
180 integer iFirstIndex = iGetFirstIndex();
181
182 /*
183 * | Delta_vP | | -v+a^T*xP |
184 * | Delta_y1P | | -y1P-O^2/T*y2 |
185 * jac * | Delta_y2P | = | -y2P+z-(2*C*O+1/T)*y2 |
186 * | Delta_zP | | -zP+y1-O*(O+2*C/T)*y2+K*O^2*vP |
187 * | Delta_xP |
188 * | Delta_gP |
189 *
190 * per il significato dei termini vedi AssJac
191 */
192
193 WorkVec.PutRowIndex(1, iFirstIndex + 1);
194 WorkVec.PutRowIndex(2, iFirstIndex + 2);
195 WorkVec.PutRowIndex(3, iFirstIndex + 3);
196 WorkVec.PutRowIndex(4, iAbstractIndex + 1);
197
198 Vec3 XP(pStrNode->GetVCurr());
199 Mat3x3 R(pStrNode->GetRCurr());
200 doublereal v = XCurr(iFirstIndex + 1);
201 doublereal vp = XPrimeCurr(iFirstIndex + 1);
202 doublereal y1 = XCurr(iFirstIndex + 2);
203 doublereal y1p = XPrimeCurr(iFirstIndex + 2);
204 doublereal y2 = XCurr(iFirstIndex + 3);
205 doublereal y2p = XPrimeCurr(iFirstIndex + 3);
206 doublereal z = XCurr(iAbstractIndex + 1);
207 doublereal zp = XPrimeCurr(iAbstractIndex + 1);
208
209 WorkVec.PutCoef(1, (R*Dir).Dot(XP) - v);
210 WorkVec.PutCoef(2, -y1p - dOmega*dOmega/dTau*y2);
211 WorkVec.PutCoef(3, -y2p + z - (2.*dCsi*dOmega + 1./dTau)*y2);
212 WorkVec.PutCoef(4, -zp + y1 - dOmega*(dOmega + 2.*dCsi/dTau)*y2
213 + dKappa*dOmega*dOmega*vp);
214
215 return WorkVec;
216 }
217
218 unsigned int
iGetNumDof(void) const219 Accelerometer::iGetNumDof(void) const
220 {
221 return 3;
222 }
223
224 DofOrder::Order
GetDofType(unsigned int i) const225 Accelerometer::GetDofType(unsigned int i) const
226 {
227 ASSERT(i >= 0 && i < 3);
228 return DofOrder::DIFFERENTIAL;
229 }
230
231 void
WorkSpaceDim(integer * piNumRows,integer * piNumCols) const232 Accelerometer::WorkSpaceDim(integer* piNumRows, integer* piNumCols) const
233 {
234 *piNumRows = 4;
235 *piNumCols = 10;
236 }
237
238 void
SetInitialValue(VectorHandler &)239 Accelerometer::SetInitialValue(VectorHandler& /* X */ )
240 {
241 NO_OP;
242 }
243
244 void
SetValue(DataManager * pDM,VectorHandler & X,VectorHandler &,SimulationEntity::Hints * ph)245 Accelerometer::SetValue(DataManager *pDM,
246 VectorHandler& X, VectorHandler& /* XP */ ,
247 SimulationEntity::Hints *ph)
248 {
249 doublereal v = (pStrNode->GetRCurr()*Dir).Dot(pStrNode->GetVCurr());
250 X.PutCoef(iGetFirstIndex() + 1, v);
251 }
252
253 /* Accelerometer - end */
254
255
256 /* TranslAccel - begin */
257
258 /* Costruttore */
TranslAccel(unsigned int uL,const DofOwner * pDO,const StructNode * pS,const ScalarDifferentialNode * pA,const Vec3 & TmpDir,const Vec3 & Tmpf,flag fOut)259 TranslAccel::TranslAccel(unsigned int uL,
260 const DofOwner* pDO,
261 const StructNode* pS,
262 const ScalarDifferentialNode* pA,
263 const Vec3& TmpDir,
264 const Vec3& Tmpf,
265 flag fOut)
266 : Elem(uL, fOut),
267 Electric(uL, pDO, fOut),
268 pStrNode(pS), pAbsNode(pA),
269 Dir(TmpDir), f(Tmpf)
270 {
271 ASSERT(pStrNode != NULL);
272 ASSERT(pStrNode->GetNodeType() == Node::STRUCTURAL);
273 ASSERT(pAbsNode != NULL);
274 ASSERT(pAbsNode->GetNodeType() == Node::ABSTRACT);
275 }
276
277 /* Distruttore banale */
~TranslAccel(void)278 TranslAccel::~TranslAccel(void)
279 {
280 NO_OP;
281 }
282
283 /* Contributo al file di restart */
284 std::ostream&
Restart(std::ostream & out) const285 TranslAccel::Restart(std::ostream& out) const
286 {
287 Electric::Restart(out) << ", accelerometer, translational, "
288 << pStrNode->GetLabel() << ", "
289 << pAbsNode->GetLabel() << ", "
290 "reference, node, ", Dir.Write(out, ", ") << ", "
291 "reference, node, ", f.Write(out, ", ") << ';'
292 << std::endl;
293 return out;
294 }
295
296 /* Costruisce il contributo allo jacobiano */
297 VariableSubMatrixHandler&
AssJac(VariableSubMatrixHandler & WorkMat,doublereal dCoef,const VectorHandler &,const VectorHandler &)298 TranslAccel::AssJac(VariableSubMatrixHandler& WorkMat,
299 doublereal dCoef,
300 const VectorHandler& /* XCurr */ ,
301 const VectorHandler& /* XPrimeCurr */ )
302 {
303 DEBUGCOUT("Entering TranslAccel::AssJac()" << std::endl);
304
305 /* Casting di WorkMat */
306 SparseSubMatrixHandler& WM = WorkMat.SetSparse();
307 WM.ResizeReset(9, 0);
308
309 /* Indici delle equazioni */
310 integer iFirstColIndex = pStrNode->iGetFirstColIndex();
311 integer iAbstractIndex = pAbsNode->iGetFirstIndex() + 1;
312 integer iFirstIndex = iGetFirstIndex() + 1;
313
314 WM.PutItem(1, iAbstractIndex, iAbstractIndex, dCoef);
315 WM.PutItem(2, iAbstractIndex, iFirstIndex, -1.);
316 WM.PutItem(3, iFirstIndex, iFirstIndex, dCoef);
317
318 Vec3 tmpf = pStrNode->GetRCurr()*f;
319 Vec3 tmpd = pStrNode->GetRCurr()*Dir;
320 Vec3 tmp = tmpf.Cross(tmpd); // FIXME?
321 WM.PutItem(4, iFirstIndex, iFirstColIndex + 1, -tmpd.dGet(1));
322 WM.PutItem(5, iFirstIndex, iFirstColIndex + 2, -tmpd.dGet(2));
323 WM.PutItem(6, iFirstIndex, iFirstColIndex + 3, -tmpd.dGet(3));
324
325 tmp = tmpd.Cross((pStrNode->GetVCurr()*(-dCoef) + tmpf));
326 WM.PutItem(7, iFirstIndex, iFirstColIndex + 4, tmp.dGet(1));
327 WM.PutItem(8, iFirstIndex, iFirstColIndex + 5, tmp.dGet(2));
328 WM.PutItem(9, iFirstIndex, iFirstColIndex + 6, tmp.dGet(3));
329
330 return WorkMat;
331 }
332
333 /* Costruisce il contributo al residuo */
334 SubVectorHandler&
AssRes(SubVectorHandler & WorkVec,doublereal,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)335 TranslAccel::AssRes(SubVectorHandler& WorkVec,
336 doublereal /* dCoef */ ,
337 const VectorHandler& XCurr,
338 const VectorHandler& XPrimeCurr)
339 {
340 DEBUGCOUT("Entering TranslAccel::AssRes()" << std::endl);
341
342 /* Dimensiona e resetta la matrice di lavoro */
343 WorkVec.Resize(2);
344
345 integer iAbstractIndex = pAbsNode->iGetFirstIndex() + 1;
346 integer iFirstIndex = iGetFirstIndex()+1;
347
348 WorkVec.PutRowIndex(1, iAbstractIndex);
349 WorkVec.PutRowIndex(2, iFirstIndex);
350
351 Vec3 tmpf = pStrNode->GetRCurr()*f;
352 Vec3 tmpd = pStrNode->GetRCurr()*Dir;
353
354 doublereal v = XCurr(iFirstIndex);
355 doublereal vp = XPrimeCurr(iFirstIndex);
356 doublereal a = pAbsNode->dGetX();
357
358 WorkVec.PutCoef(1, vp - a);
359 WorkVec.PutCoef(2, tmpd.Dot((pStrNode->GetVCurr() + pStrNode->GetWCurr().Cross(tmpf))) - v);
360
361 return WorkVec;
362 }
363
364 unsigned int
iGetNumDof(void) const365 TranslAccel::iGetNumDof(void) const
366 {
367 return 1;
368 }
369
370 DofOrder::Order
GetDofType(unsigned int i) const371 TranslAccel::GetDofType(unsigned int i) const
372 {
373 ASSERT(i == 0);
374 return DofOrder::DIFFERENTIAL;
375 }
376
377 void
WorkSpaceDim(integer * piNumRows,integer * piNumCols) const378 TranslAccel::WorkSpaceDim(integer* piNumRows, integer* piNumCols) const
379 {
380 *piNumRows = 2;
381 *piNumCols = 8;
382 }
383
384 void
SetInitialValue(VectorHandler &)385 TranslAccel::SetInitialValue(VectorHandler& /* X */ )
386 {
387 NO_OP;
388 }
389
390 void
SetValue(DataManager * pDM,VectorHandler & X,VectorHandler & XP,SimulationEntity::Hints * ph)391 TranslAccel::SetValue(DataManager *pDM,
392 VectorHandler& X, VectorHandler& XP,
393 SimulationEntity::Hints *ph)
394 {
395 doublereal v =
396 (pStrNode->GetRCurr()*Dir).Dot(pStrNode->GetVCurr()
397 + pStrNode->GetWCurr().Cross(pStrNode->GetRCurr()*f));
398 X.PutCoef(iGetFirstIndex() + 1, v);
399 XP.PutCoef(iGetFirstIndex() + 1, 0.);
400 }
401
402 /* TranslAccel - end */
403
404
405 /* RotAccel - begin */
406
407 /* Costruttore */
RotAccel(unsigned int uL,const DofOwner * pDO,const StructNode * pS,const ScalarDifferentialNode * pA,const Vec3 & TmpDir,flag fOut)408 RotAccel::RotAccel(unsigned int uL,
409 const DofOwner* pDO,
410 const StructNode* pS,
411 const ScalarDifferentialNode* pA,
412 const Vec3& TmpDir,
413 flag fOut)
414 : Elem(uL, fOut),
415 Electric(uL, pDO, fOut),
416 pStrNode(pS), pAbsNode(pA),
417 Dir(TmpDir)
418 {
419 ASSERT(pStrNode != NULL);
420 ASSERT(pStrNode->GetNodeType() == Node::STRUCTURAL);
421 ASSERT(pAbsNode != NULL);
422 ASSERT(pAbsNode->GetNodeType() == Node::ABSTRACT);
423 }
424
425 /* Distruttore banale */
~RotAccel(void)426 RotAccel::~RotAccel(void)
427 {
428 NO_OP;
429 }
430
431 /* Contributo al file di restart */
432 std::ostream&
Restart(std::ostream & out) const433 RotAccel::Restart(std::ostream& out) const
434 {
435 Electric::Restart(out) << ", accelerometer, rotational, "
436 << pStrNode->GetLabel() << ", "
437 << pAbsNode->GetLabel() << ", "
438 "reference, node, ", Dir.Write(out, ", ") << ';'
439 << std::endl;
440 return out;
441 }
442
443 /* Costruisce il contributo allo jacobiano */
444 VariableSubMatrixHandler&
AssJac(VariableSubMatrixHandler & WorkMat,doublereal dCoef,const VectorHandler &,const VectorHandler &)445 RotAccel::AssJac(VariableSubMatrixHandler& WorkMat,
446 doublereal dCoef,
447 const VectorHandler& /* XCurr */ ,
448 const VectorHandler& /* XPrimeCurr */ )
449 {
450 DEBUGCOUT("Entering RotAccel::AssJac()" << std::endl);
451
452 /* Casting di WorkMat */
453 SparseSubMatrixHandler& WM = WorkMat.SetSparse();
454 WM.ResizeReset(6, 0);
455
456 /* Indici delle equazioni */
457 integer iFirstColIndex = pStrNode->iGetFirstColIndex();
458 integer iAbstractIndex = pAbsNode->iGetFirstIndex()+1;
459 integer iFirstIndex = iGetFirstIndex()+1;
460
461 WM.PutItem(1, iAbstractIndex, iAbstractIndex, dCoef);
462 WM.PutItem(2, iAbstractIndex, iFirstIndex, -1.);
463 WM.PutItem(3, iFirstIndex, iFirstIndex, dCoef);
464
465 Vec3 tmp = pStrNode->GetRCurr()*Dir;
466 WM.PutItem(4, iFirstIndex, iFirstColIndex+4, tmp.dGet(1));
467 WM.PutItem(5, iFirstIndex, iFirstColIndex+5, tmp.dGet(2));
468 WM.PutItem(6, iFirstIndex, iFirstColIndex+6, tmp.dGet(3));
469
470 return WorkMat;
471 }
472
473 /* Costruisce il contributo al residuo */
474 SubVectorHandler&
AssRes(SubVectorHandler & WorkVec,doublereal,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)475 RotAccel::AssRes(SubVectorHandler& WorkVec,
476 doublereal /* dCoef */ ,
477 const VectorHandler& XCurr,
478 const VectorHandler& XPrimeCurr)
479 {
480 DEBUGCOUT("Entering RotAccel::AssRes()" << std::endl);
481
482 /* Dimensiona e resetta la matrice di lavoro */
483 WorkVec.Resize(2);
484
485 integer iAbstractIndex = pAbsNode->iGetFirstIndex() + 1;
486 integer iFirstIndex = iGetFirstIndex() + 1;
487
488 WorkVec.PutRowIndex(1, iAbstractIndex);
489 WorkVec.PutRowIndex(2, iFirstIndex);
490
491 Vec3 tmp = pStrNode->GetRCurr()*Dir;
492
493 doublereal v = XCurr(iFirstIndex);
494 doublereal vp = XPrimeCurr(iFirstIndex);
495 doublereal a = pAbsNode->dGetX();
496
497 WorkVec.PutCoef(1, vp - a);
498 WorkVec.PutCoef(2, tmp.Dot(pStrNode->GetWCurr()) - v);
499
500 return WorkVec;
501 }
502
503 unsigned int
iGetNumDof(void) const504 RotAccel::iGetNumDof(void) const
505 {
506 return 1;
507 }
508
509 DofOrder::Order
GetDofType(unsigned int i) const510 RotAccel::GetDofType(unsigned int i) const
511 {
512 ASSERT(i == 0);
513 return DofOrder::DIFFERENTIAL;
514 }
515
516 void
WorkSpaceDim(integer * piNumRows,integer * piNumCols) const517 RotAccel::WorkSpaceDim(integer* piNumRows, integer* piNumCols) const
518 {
519 *piNumRows = 2;
520 *piNumCols = 5;
521 }
522
523 void
SetInitialValue(VectorHandler &)524 RotAccel::SetInitialValue(VectorHandler& /* X */ )
525 {
526 NO_OP;
527 }
528
529 void
SetValue(DataManager * pDM,VectorHandler & X,VectorHandler &,SimulationEntity::Hints * ph)530 RotAccel::SetValue(DataManager *pDM,
531 VectorHandler& X, VectorHandler& /* XP */ ,
532 SimulationEntity::Hints *ph)
533 {
534 doublereal v = (pStrNode->GetRCurr()*Dir).Dot(pStrNode->GetWCurr());
535 X.PutCoef(iGetFirstIndex() + 1, v);
536 }
537
538 /* RotAccel - end */
539
540