1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/rodj.cc,v 1.71 2017/05/12 17:29:26 morandini Exp $ */
2 /*
3 * MBDyn (C) is a multibody analysis code.
4 * http://www.mbdyn.org
5 *
6 * Copyright (C) 1996-2014
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 /* Rods */
33
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35
36 #include <limits>
37 #include <cmath>
38 #include <limits>
39
40 #include "dataman.h"
41 #include "rodj.h"
42
43 /* Rod - begin */
44
45 /* Costruttore non banale */
Rod(unsigned int uL,const DofOwner * pDO,const ConstitutiveLaw1D * pCL,const StructDispNode * pN1,const StructDispNode * pN2,doublereal dLength,flag fOut,bool bHasOffsets)46 Rod::Rod(unsigned int uL, const DofOwner* pDO,
47 const ConstitutiveLaw1D* pCL,
48 const StructDispNode* pN1, const StructDispNode* pN2,
49 doublereal dLength, flag fOut, bool bHasOffsets)
50 : Elem(uL, fOut),
51 Joint(uL, pDO, fOut),
52 ConstitutiveLaw1DOwner(pCL),
53 pNode1(pN1),
54 pNode2(pN2),
55 dL0(dLength),
56 v(Zero3),
57 dElle(0.),
58 dEpsilon(0.),
59 dEpsilonPrime(0.)
60 #ifdef USE_NETCDF
61 ,
62 Var_v(0),
63 Var_dElle(0),
64 Var_dEllePrime(0)
65 #endif // USE_NETCDF
66 {
67 /* Verifica di consistenza dei dati iniziali */
68 ASSERT(pNode1 != 0);
69 ASSERT(pNode1->GetNodeType() == Node::STRUCTURAL);
70 ASSERT(pNode2 != 0);
71 ASSERT(pNode2->GetNodeType() == Node::STRUCTURAL);
72
73 if (!bHasOffsets) {
74 v = pNode2->GetXCurr() - pNode1->GetXCurr();
75
76 doublereal dDot = v.Dot();
77 if (dDot <= std::numeric_limits<doublereal>::epsilon()) {
78 silent_cerr("Rod(" << GetLabel() << "): "
79 "initial length must be non-null" << std::endl);
80 throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
81 }
82
83 dElle = std::sqrt(dDot);
84 }
85
86 ASSERT(dLength > std::numeric_limits<doublereal>::epsilon());
87 }
88
89 /* Distruttore */
~Rod(void)90 Rod::~Rod(void)
91 {
92 NO_OP;
93 }
94
95 /* Deformazione */
96 doublereal
dCalcEpsilon(void)97 Rod::dCalcEpsilon(void)
98 {
99 return dElle/dL0 - 1.;
100 }
101
102
103 void
AfterConvergence(const VectorHandler & X,const VectorHandler & XP)104 Rod::AfterConvergence(const VectorHandler& X, const VectorHandler& XP)
105 {
106 ConstitutiveLaw1DOwner::AfterConvergence(dEpsilon);
107 }
108
109 /* Contributo al file di restart */
110 std::ostream&
Restart(std::ostream & out) const111 Rod::Restart(std::ostream& out) const
112 {
113 Joint::Restart(out) << ", rod, "
114 << pNode1->GetLabel() << ", "
115 << pNode2->GetLabel() << ", "
116 << dL0 << ", ";
117 return pGetConstLaw()->Restart(out) << ';' << std::endl;
118 }
119
120 void
AssMat(FullSubMatrixHandler & WorkMat,doublereal dCoef)121 Rod::AssMat(FullSubMatrixHandler& WorkMat, doublereal dCoef)
122 {
123 /* v = x2-x1 */
124 /* v = pNode2->GetXCurr()-pNode1->GetXCurr(); */
125 doublereal dCross = v.Dot();
126
127 /* Verifica che la distanza non sia nulla */
128 if (dCross <= std::numeric_limits<doublereal>::epsilon()) {
129 silent_cerr("Rod(" << GetLabel() << "): "
130 "null distance between nodes " << pNode1->GetLabel()
131 << " and " << pNode2->GetLabel() << std::endl);
132 throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
133 }
134
135 /* Lunghezza corrente */
136 dElle = std::sqrt(dCross);
137
138 /* Forza e slope */
139 doublereal dF = GetF();
140 doublereal dFDE = GetFDE();
141
142 Mat3x3 K(Mat3x3(MatCrossCross, v, v*((-dF*dCoef)/(dElle*dCross)))
143 +v.Tens(v*((dFDE*dCoef)/(dL0*dCross))));
144
145 /* Termini diagonali */
146 WorkMat.Add(1, 1, K);
147 WorkMat.Add(4, 4, K);
148
149 /* termini extradiagonali */
150 WorkMat.Sub(1, 4, K);
151 WorkMat.Sub(4, 1, K);
152 }
153
154 void
AssVec(SubVectorHandler & WorkVec)155 Rod::AssVec(SubVectorHandler& WorkVec)
156 {
157 /* v = x2-x1 */
158 v = pNode2->GetXCurr() - pNode1->GetXCurr();
159 doublereal dCross = v.Dot();
160
161 /* Verifica che la distanza non sia nulla */
162 if (dCross <= std::numeric_limits<doublereal>::epsilon()) {
163 silent_cerr("Rod(" << GetLabel() << "): "
164 "null distance between nodes " << pNode1->GetLabel()
165 << " and " << pNode2->GetLabel() << std::endl);
166 throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
167 }
168
169 /* Deformazione */
170 dElle = sqrt(dCross);
171 dEpsilon = dCalcEpsilon();
172
173 Vec3 vPrime(pNode2->GetVCurr() - pNode1->GetVCurr());
174 dEpsilonPrime = v.Dot(vPrime)/(dElle*dL0);
175
176 /* Ampiezza della forza */
177 bool ChangeJac(false);
178 try {
179 ConstitutiveLaw1DOwner::Update(dEpsilon);
180
181 } catch (Elem::ChangedEquationStructure) {
182 ChangeJac = true;
183 }
184
185 doublereal dF = GetF();
186
187 /* Vettore forza */
188 Vec3 F = v*(dF/dElle);
189
190 WorkVec.Add(1, F);
191 WorkVec.Sub(4, F);
192
193 if (ChangeJac) {
194 throw Elem::ChangedEquationStructure(MBDYN_EXCEPT_ARGS);
195 }
196 }
197
198 VariableSubMatrixHandler&
AssJac(VariableSubMatrixHandler & WorkMat,doublereal dCoef,const VectorHandler &,const VectorHandler &)199 Rod::AssJac(VariableSubMatrixHandler& WorkMat,
200 doublereal dCoef,
201 const VectorHandler& /* XCurr */ ,
202 const VectorHandler& /* XPrimeCurr */ )
203 {
204 DEBUGCOUT("Entering Rod::AssJac()" << std::endl);
205
206 FullSubMatrixHandler& WM = WorkMat.SetFull();
207
208 /* Dimensiona e resetta la matrice di lavoro */
209 integer iNumRows = 0;
210 integer iNumCols = 0;
211 WorkSpaceDim(&iNumRows, &iNumCols);
212 WM.ResizeReset(iNumRows, iNumCols);
213
214 /* Recupera gli indici */
215 integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
216 integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
217 integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
218 integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
219
220 /* Setta gli indici della matrice */
221 for (int iCnt = 1; iCnt <= 3; iCnt++) {
222 WM.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
223 WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
224 WM.PutRowIndex(3 + iCnt, iNode2FirstMomIndex + iCnt);
225 WM.PutColIndex(3 + iCnt, iNode2FirstPosIndex + iCnt);
226 }
227
228 /* Genera la matrice */
229 AssMat(WM, dCoef);
230
231 return WorkMat;
232 }
233
234 void
AssMats(VariableSubMatrixHandler & WorkMatA,VariableSubMatrixHandler & WorkMatB,const VectorHandler &,const VectorHandler &)235 Rod::AssMats(VariableSubMatrixHandler& WorkMatA,
236 VariableSubMatrixHandler& WorkMatB,
237 const VectorHandler& /* XCurr */ ,
238 const VectorHandler& /* XPrimeCurr */ )
239 {
240 DEBUGCOUT("Entering Rod::AssMats()" << std::endl);
241
242 WorkMatB.SetNullMatrix();
243 FullSubMatrixHandler& WMA = WorkMatA.SetFull();
244
245 /* Dimensiona e resetta la matrice di lavoro */
246 integer iNumRows = 0;
247 integer iNumCols = 0;
248 WorkSpaceDim(&iNumRows, &iNumCols);
249 WMA.ResizeReset(iNumRows, iNumCols);
250
251 /* Recupera gli indici */
252 integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
253 integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
254 integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
255 integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
256
257 /* Setta gli indici della matrice */
258 for (int iCnt = 1; iCnt <= 3; iCnt++) {
259 WMA.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
260 WMA.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
261 WMA.PutRowIndex(3 + iCnt, iNode2FirstMomIndex + iCnt);
262 WMA.PutColIndex(3 + iCnt, iNode2FirstPosIndex + iCnt);
263 }
264
265 /* Genera la matrice */
266 AssMat(WMA, 1.);
267 }
268
269 SubVectorHandler&
AssRes(SubVectorHandler & WorkVec,doublereal,const VectorHandler &,const VectorHandler &)270 Rod::AssRes(SubVectorHandler& WorkVec,
271 doublereal /* dCoef */ ,
272 const VectorHandler& /* XCurr */ ,
273 const VectorHandler& /* XPrimeCurr */ )
274 {
275 DEBUGCOUT("Entering Rod::AssRes()" << std::endl);
276
277 /* Dimensiona e resetta la matrice di lavoro */
278 integer iNumRows = 0;
279 integer iNumCols = 0;
280 WorkSpaceDim(&iNumRows, &iNumCols);
281 WorkVec.ResizeReset(iNumRows);
282
283 /* Indici */
284 integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
285 integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
286
287 /* Setta gli indici */
288 for (int iCnt = 1; iCnt <= 3; iCnt++) {
289 WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
290 WorkVec.PutRowIndex(3 + iCnt, iNode2FirstMomIndex + iCnt);
291 }
292
293 /* Costruisce il vettore */
294 AssVec(WorkVec);
295
296 return WorkVec;
297 }
298
299 void
OutputPrepare(OutputHandler & OH)300 Rod::OutputPrepare(OutputHandler& OH)
301 {
302 if (bToBeOutput()) {
303 #ifdef USE_NETCDF
304 if (OH.UseNetCDF(OutputHandler::JOINTS)) {
305 std::string name;
306 OutputPrepare_int("rod", OH, name);
307
308 Var_dElle = OH.CreateVar<doublereal>(name + "l", "m",
309 "length of the element");
310 Var_dEllePrime = OH.CreateVar<doublereal>(name + "lP", "m/2",
311 "lengthening velocity of the element");
312 Var_v = OH.CreateVar<Vec3>(name + "v", "m",
313 "direction unit vector");
314 }
315 #endif // USE_NETCDF
316 }
317 }
318
319 void
Output(OutputHandler & OH) const320 Rod::Output(OutputHandler& OH) const
321 {
322 if (bToBeOutput()) {
323 ASSERT(dElle > std::numeric_limits<doublereal>::epsilon());
324
325
326 Vec3 vTmp(v/dElle);
327 doublereal d = GetF();
328 doublereal dEllePrime = dEpsilonPrime*dL0;
329
330 #ifdef USE_NETCDF
331 if (OH.UseNetCDF(OutputHandler::JOINTS)) {
332
333 Vec3 F = Vec3(d, 0., 0.);
334 Vec3 M = Zero3;
335 Vec3 FTmp = vTmp*d;
336
337 Var_F_local->put_rec(F.pGetVec(), OH.GetCurrentStep());
338 Var_M_local->put_rec(M.pGetVec(), OH.GetCurrentStep());
339 Var_F_global->put_rec(FTmp.pGetVec(), OH.GetCurrentStep());
340 Var_M_global->put_rec(M.pGetVec(), OH.GetCurrentStep());
341 Var_dElle->put_rec(&dElle, OH.GetCurrentStep());
342 Var_dEllePrime->put_rec(&dEllePrime, OH.GetCurrentStep());
343 Var_v->put_rec(vTmp.pGetVec(), OH.GetCurrentStep());
344 }
345 #endif // USE_NETCDF
346
347 std::ostream& out = OH.Joints();
348
349 Joint::Output(out, "Rod", GetLabel(),
350 Vec3(d, 0., 0.), Zero3, vTmp*d, Zero3)
351 << " " << dElle << " " << vTmp << " " << dEpsilonPrime*dL0,
352 ConstitutiveLaw1DOwner::OutputAppend(out) << std::endl;
353 }
354 }
355
356 VariableSubMatrixHandler&
InitialAssJac(VariableSubMatrixHandler & WorkMat,const VectorHandler &)357 Rod::InitialAssJac(VariableSubMatrixHandler& WorkMat,
358 const VectorHandler& /* XCurr */ )
359 {
360 DEBUGCOUT("Entering Rod::InitialAssJac()" << std::endl);
361
362 FullSubMatrixHandler& WM = WorkMat.SetFull();
363
364 /* Dimensiona e resetta la matrice di lavoro */
365 integer iNumRows = 0;
366 integer iNumCols = 0;
367 InitialWorkSpaceDim(&iNumRows, &iNumCols);
368 WM.ResizeReset(iNumRows, iNumCols);
369
370 /* Recupera gli indici */
371 integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
372 integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
373
374 /* Setta gli indici della matrice */
375 for (int iCnt = 1; iCnt <= 3; iCnt++) {
376 WM.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
377 WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
378 WM.PutRowIndex(3 + iCnt, iNode2FirstPosIndex + iCnt);
379 WM.PutColIndex(3 + iCnt, iNode2FirstPosIndex + iCnt);
380 }
381
382 /* Genera la matrice */
383 AssMat(WM);
384
385 return WorkMat;
386 }
387
388 SubVectorHandler&
InitialAssRes(SubVectorHandler & WorkVec,const VectorHandler &)389 Rod::InitialAssRes(SubVectorHandler& WorkVec,
390 const VectorHandler& /* XCurr */ )
391 {
392 DEBUGCOUT("Entering Rod::InitialAssRes()" << std::endl);
393
394 /* Dimensiona e resetta la matrice di lavoro */
395 integer iNumRows = 0;
396 integer iNumCols = 0;
397 InitialWorkSpaceDim(&iNumRows, &iNumCols);
398 WorkVec.ResizeReset(iNumRows);
399
400 /* Indici */
401 integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
402 integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
403
404 /* Setta gli indici */
405 for (int iCnt = 1; iCnt <= 3; iCnt++) {
406 WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
407 WorkVec.PutRowIndex(3 + iCnt, iNode2FirstPosIndex + iCnt);
408 }
409
410 /* Costruisce il vettore */
411 AssVec(WorkVec);
412
413 return WorkVec;
414 }
415
416 /* inverse dynamics capable element */
417 bool
bInverseDynamics(void) const418 Rod::bInverseDynamics(void) const
419 {
420 return true;
421 }
422
423
424 /* Inverse Dynamics Jacobian matrix assembly */
425 VariableSubMatrixHandler&
AssJac(VariableSubMatrixHandler & WorkMat,const VectorHandler & XCurr)426 Rod::AssJac(VariableSubMatrixHandler& WorkMat,
427 const VectorHandler& XCurr)
428 {
429 ASSERT(bIsErgonomy());
430
431 return AssJac(WorkMat, 1., XCurr, XCurr);
432 }
433
434 /* Inverse Dynamics Residual Assembly */
435 SubVectorHandler&
AssRes(SubVectorHandler & WorkVec,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr,const VectorHandler &,InverseDynamics::Order iOrder)436 Rod::AssRes(SubVectorHandler& WorkVec,
437 const VectorHandler& XCurr,
438 const VectorHandler& XPrimeCurr,
439 const VectorHandler& /* XPrimePrimeCurr */ ,
440 InverseDynamics::Order iOrder)
441 {
442 DEBUGCOUT("Entering Rod::AssRes()" << std::endl);
443
444 ASSERT(iOrder == InverseDynamics::INVERSE_DYNAMICS
445 || (iOrder == InverseDynamics::POSITION && bIsErgonomy()));
446
447 return AssRes(WorkVec, 1., XCurr, XPrimeCurr);
448 }
449
450 /* Inverse Dynamics update */
451 void
Update(const VectorHandler & XCurr,InverseDynamics::Order iOrder)452 Rod::Update(const VectorHandler& XCurr, InverseDynamics::Order iOrder)
453 {
454 NO_OP;
455 }
456
457 /* Inverse Dynamics after convergence */
458 void
AfterConvergence(const VectorHandler & X,const VectorHandler & XP,const VectorHandler & XPP)459 Rod::AfterConvergence(const VectorHandler& X,
460 const VectorHandler& XP, const VectorHandler& XPP)
461 {
462 AfterConvergence(X, XP);
463 }
464
465
466 unsigned int
iGetNumPrivData(void) const467 Rod::iGetNumPrivData(void) const
468 {
469 return 3 + ConstitutiveLaw1DOwner::iGetNumPrivData();
470 }
471
472 unsigned int
iGetPrivDataIdx(const char * s) const473 Rod::iGetPrivDataIdx(const char *s) const
474 {
475 ASSERT(s != NULL);
476
477 if (strcmp(s, "F") == 0) {
478 return 1;
479 }
480
481 if (strcmp(s, "L") == 0) {
482 return 2;
483 }
484
485 if (strcmp(s, "LPrime") == 0) {
486 return 3;
487 }
488
489 size_t l = STRLENOF("constitutiveLaw.");
490 if (strncmp(s, "constitutiveLaw.", l) == 0) {
491 return 3 + ConstitutiveLaw1DOwner::iGetPrivDataIdx(&s[l]);
492 }
493
494 /* error; handle later */
495 return 0;
496 }
497
498 doublereal
dGetPrivData(unsigned int i) const499 Rod::dGetPrivData(unsigned int i) const
500 {
501 ASSERT(i > 0);
502
503 switch (i) {
504 case 1:
505 return GetF();
506
507 case 2:
508 return dElle;
509
510 case 3:
511 return dL0*dEpsilonPrime;
512 }
513
514 i -= 3;
515 ASSERT(i <= ConstitutiveLaw1DOwner::iGetNumPrivData());
516
517 return ConstitutiveLaw1DOwner::dGetPrivData(i);
518 }
519
520 /* Rod - end */
521
522
523 /* ViscoElasticRod - begin */
524
525 /* Costruttore non banale */
ViscoElasticRod(unsigned int uL,const DofOwner * pDO,const ConstitutiveLaw1D * pCL,const StructDispNode * pN1,const StructDispNode * pN2,doublereal dLength,flag fOut)526 ViscoElasticRod::ViscoElasticRod(unsigned int uL,
527 const DofOwner* pDO,
528 const ConstitutiveLaw1D* pCL,
529 const StructDispNode* pN1,
530 const StructDispNode* pN2,
531 doublereal dLength, flag fOut)
532 : Elem(uL, fOut),
533 Rod(uL, pDO, pCL, pN1, pN2, dLength, fOut)
534 {
535 NO_OP;
536 }
537
538 /* Distruttore */
~ViscoElasticRod(void)539 ViscoElasticRod::~ViscoElasticRod(void)
540 {
541 NO_OP;
542 }
543
544 void
AfterConvergence(const VectorHandler & X,const VectorHandler & XP)545 ViscoElasticRod::AfterConvergence(const VectorHandler& X,
546 const VectorHandler& XP)
547 {
548 ConstitutiveLaw1DOwner::AfterConvergence(dEpsilon, dEpsilonPrime);
549 }
550
551 VariableSubMatrixHandler&
AssJac(VariableSubMatrixHandler & WorkMat,doublereal dCoef,const VectorHandler &,const VectorHandler &)552 ViscoElasticRod::AssJac(VariableSubMatrixHandler& WorkMat,
553 doublereal dCoef,
554 const VectorHandler& /* XCurr */ ,
555 const VectorHandler& /* XPrimeCurr */ )
556 {
557 DEBUGCOUT("Entering ViscoElasticRod::AssJac()" << std::endl);
558
559 FullSubMatrixHandler& WM = WorkMat.SetFull();
560
561 /* Dimensiona e resetta la matrice di lavoro */
562 integer iNumRows = 0;
563 integer iNumCols = 0;
564 WorkSpaceDim(&iNumRows, &iNumCols);
565 WM.ResizeReset(iNumRows, iNumCols);
566
567 /* Recupera gli indici */
568 integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
569 integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
570 integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
571 integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
572
573 /* Setta gli indici della matrice */
574 for (int iCnt = 1; iCnt <= 3; iCnt++) {
575 WM.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
576 WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
577 WM.PutRowIndex(3 + iCnt, iNode2FirstMomIndex + iCnt);
578 WM.PutColIndex(3 + iCnt, iNode2FirstPosIndex + iCnt);
579 }
580
581 /* v = x2-x1 */
582 /* v(pNode2->GetXCurr()-pNode1->GetXCurr()); */
583 doublereal dCross = v.Dot();
584
585 /* Verifica che la distanza non sia nulla */
586 if (dCross <= std::numeric_limits<doublereal>::epsilon()) {
587 silent_cerr("ViscoElasticRod(" << GetLabel() << "): "
588 "null distance between nodes " << pNode1->GetLabel()
589 << " and " << pNode2->GetLabel() << std::endl);
590 throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
591 }
592
593 /* Lunghezza corrente */
594 dElle = sqrt(dCross);
595
596 /* Velocita' di deformazione */
597 Vec3 vPrime(pNode2->GetVCurr()-pNode1->GetVCurr());
598
599 /* Forza e slopes */
600 doublereal dF = GetF();
601 doublereal dFDE = GetFDE();
602 doublereal dFDEPrime = GetFDEPrime();
603
604 Mat3x3 K(Mat3x3( MatCrossCross, v, v*((-dF*dCoef)/(dElle*dCross)) )
605 + v.Tens( v*((dFDE*dCoef+dFDEPrime)/(dL0*dCross)) )
606 + v.Tens( v.Cross( vPrime.Cross( v*((dFDEPrime*dCoef)/(dL0*dCross*dCross)) ) ) ));
607
608 /* Termini diagonali */
609 WM.Add(1, 1, K);
610 WM.Add(4, 4, K);
611
612 /* termini extradiagonali */
613 WM.Sub(1, 4, K);
614 WM.Sub(4, 1, K);
615
616 return WorkMat;
617 }
618
619 SubVectorHandler&
AssRes(SubVectorHandler & WorkVec,doublereal,const VectorHandler &,const VectorHandler &)620 ViscoElasticRod::AssRes(SubVectorHandler& WorkVec,
621 doublereal /* dCoef */ ,
622 const VectorHandler& /* XCurr */ ,
623 const VectorHandler& /* XPrimeCurr */ )
624 {
625 DEBUGCOUT("Entering ViscoElasticRod::AssRes()" << std::endl);
626
627 /* Dimensiona e resetta la matrice di lavoro */
628 integer iNumRows = 0;
629 integer iNumCols = 0;
630 WorkSpaceDim(&iNumRows, &iNumCols);
631 WorkVec.ResizeReset(iNumRows);
632
633 /* Indici */
634 integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
635 integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
636
637 /* Setta gli indici */
638 for (int iCnt = 1; iCnt <= 3; iCnt++) {
639 WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
640 WorkVec.PutRowIndex(3 + iCnt, iNode2FirstMomIndex + iCnt);
641 }
642
643 /* v = x2-x1 */
644 v = pNode2->GetXCurr()-pNode1->GetXCurr();
645 doublereal dCross = v.Dot();
646
647 /* Verifica che la distanza non sia nulla */
648 if (dCross <= std::numeric_limits<doublereal>::epsilon()) {
649 silent_cerr("ViscoElasticRod(" << GetLabel() << "): "
650 "null distance between nodes " << pNode1->GetLabel()
651 << " and " << pNode2->GetLabel() << std::endl);
652 throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
653 }
654
655 /* Lunghezza corrente */
656 dElle = sqrt(dCross);
657
658 /* Deformazione */
659 dEpsilon = dCalcEpsilon();
660
661 /* Velocita' di deformazione */
662 Vec3 vPrime(pNode2->GetVCurr() - pNode1->GetVCurr());
663 dEpsilonPrime = (v.Dot(vPrime))/(dElle*dL0);
664
665 /* Ampiezza della forza */
666 bool ChangeJac(false);
667 try {
668 ConstitutiveLaw1DOwner::Update(dEpsilon, dEpsilonPrime);
669
670 } catch (Elem::ChangedEquationStructure) {
671 ChangeJac = true;
672 }
673 doublereal dF = GetF();
674
675 /* Vettore forza */
676 Vec3 F(v*(dF/dElle));
677
678 WorkVec.Add(1, F);
679 WorkVec.Sub(4, F);
680
681 if (ChangeJac) {
682 throw Elem::ChangedEquationStructure(MBDYN_EXCEPT_ARGS);
683 }
684
685 return WorkVec;
686 }
687
688 VariableSubMatrixHandler&
InitialAssJac(VariableSubMatrixHandler & WorkMat,const VectorHandler &)689 ViscoElasticRod::InitialAssJac(VariableSubMatrixHandler& WorkMat,
690 const VectorHandler& /* XCurr */ )
691 {
692 DEBUGCOUT("Entering ViscoElasticRod::InitialAssJac()" << std::endl);
693
694 FullSubMatrixHandler& WM = WorkMat.SetFull();
695
696 /* Dimensiona e resetta la matrice di lavoro */
697 integer iNumRows = 0;
698 integer iNumCols = 0;
699 InitialWorkSpaceDim(&iNumRows, &iNumCols);
700 WM.ResizeReset(iNumRows, iNumCols);
701
702 /* Recupera gli indici */
703 integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
704 integer iNode1FirstVelIndex = iNode1FirstPosIndex+6;
705 integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
706 integer iNode2FirstVelIndex = iNode2FirstPosIndex+6;
707
708 /* Setta gli indici della matrice */
709 for (int iCnt = 1; iCnt <= 3; iCnt++) {
710 WM.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
711 WM.PutColIndex(iCnt, iNode1FirstPosIndex+iCnt);
712 WM.PutColIndex(3+iCnt, iNode1FirstVelIndex+iCnt);
713
714 WM.PutRowIndex(3+iCnt, iNode2FirstPosIndex+iCnt);
715 WM.PutColIndex(6+iCnt, iNode2FirstPosIndex+iCnt);
716 WM.PutColIndex(9+iCnt, iNode2FirstVelIndex+iCnt);
717 }
718
719 /* v = x2-x1 */
720 /* v(pNode2->GetXCurr()-pNode1->GetXCurr()); */
721 doublereal dCross = v.Dot();
722
723 /* Verifica che la distanza non sia nulla */
724 if (dCross <= std::numeric_limits<doublereal>::epsilon()) {
725 silent_cerr("ViscoElasticRod(" << GetLabel() << "): "
726 "null distance between nodes " << pNode1->GetLabel()
727 << " and " << pNode2->GetLabel() << std::endl);
728 throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
729 }
730
731 /* Lunghezza corrente */
732 dElle = sqrt(dCross);
733
734 /* Velocita' di deformazione */
735 Vec3 vPrime(pNode2->GetVCurr()-pNode1->GetVCurr());
736
737 /* Forza e slopes */
738 doublereal dF = GetF();
739 doublereal dFDE = GetFDE();
740 doublereal dFDEPrime = GetFDEPrime();
741
742 Mat3x3 K(Mat3x3(MatCrossCross, v, v*((-dF)/(dElle*dCross)))
743 + v.Tens(v*((dFDE)/(dL0*dCross)))
744 + v.Tens(v.Cross(vPrime.Cross(v*((dFDEPrime)/(dL0*dCross*dCross))))));
745 Mat3x3 KPrime(v.Tens(v*((dFDEPrime)/(dL0*dCross))));
746
747 /* Termini diagonali */
748 WM.Add(1, 1, K);
749 WM.Add(4, 7, K);
750
751 WM.Add(1, 4, KPrime);
752 WM.Add(4, 10, KPrime);
753
754 /* termini extradiagonali */
755 WM.Sub(1, 7, K);
756 WM.Sub(4, 1, K);
757
758 WM.Sub(1, 10, KPrime);
759 WM.Sub(4, 4, KPrime);
760
761 return WorkMat;
762 }
763
764 SubVectorHandler&
InitialAssRes(SubVectorHandler & WorkVec,const VectorHandler &)765 ViscoElasticRod::InitialAssRes(SubVectorHandler& WorkVec,
766 const VectorHandler& /* XCurr */ )
767 {
768 DEBUGCOUT("Entering ViscoElasticRod::InitialAssRes()" << std::endl);
769
770 /* Dimensiona e resetta la matrice di lavoro */
771 integer iNumRows = 0;
772 integer iNumCols = 0;
773 InitialWorkSpaceDim(&iNumRows, &iNumCols);
774 WorkVec.ResizeReset(iNumRows);
775
776 /* Indici */
777 integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
778 integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
779
780 /* Setta gli indici */
781 for (int iCnt = 1; iCnt <= 3; iCnt++) {
782 WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
783 WorkVec.PutRowIndex(3 + iCnt, iNode2FirstPosIndex + iCnt);
784 }
785
786 /* v = x2-x1 */
787 v = pNode2->GetXCurr() - pNode1->GetXCurr();
788 doublereal dCross = v.Dot();
789
790 /* Verifica che la distanza non sia nulla */
791 if (dCross <= std::numeric_limits<doublereal>::epsilon()) {
792 silent_cerr("ViscoElasticRod(" << GetLabel() << "): "
793 "null distance between nodes " << pNode1->GetLabel()
794 << " and " << pNode2->GetLabel() << std::endl);
795 throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
796 }
797
798 /* Lunghezza corrente */
799 dElle = sqrt(dCross);
800
801 /* Deformazione */
802 dEpsilon = dCalcEpsilon();
803
804 /* Velocita' di deformazione */
805 Vec3 vPrime(pNode2->GetVCurr() - pNode1->GetVCurr());
806 dEpsilonPrime = (v.Dot(vPrime))/(dElle*dL0);
807
808 /* Ampiezza della forza */
809 ConstitutiveLaw1DOwner::Update(dEpsilon, dEpsilonPrime);
810 doublereal dF = GetF();
811
812 /* Vettore forza */
813 Vec3 F(v*(dF/dElle));
814
815 WorkVec.Add(1, F);
816 WorkVec.Sub(4, F);
817
818 return WorkVec;
819 }
820
821 /* ViscoElasticRod - end */
822
823
824 /* RodWithOffset - begin */
825
826 /* Costruttore non banale */
RodWithOffset(unsigned int uL,const DofOwner * pDO,const ConstitutiveLaw1D * pCL,const StructNode * pN1,const StructNode * pN2,const Vec3 & f1Tmp,const Vec3 & f2Tmp,doublereal dLength,flag fOut)827 RodWithOffset::RodWithOffset(unsigned int uL,
828 const DofOwner* pDO,
829 const ConstitutiveLaw1D* pCL,
830 const StructNode* pN1,
831 const StructNode* pN2,
832 const Vec3& f1Tmp,
833 const Vec3& f2Tmp,
834 doublereal dLength,
835 flag fOut)
836 : Elem(uL, fOut),
837 Rod(uL, pDO, pCL, pN1, pN2, dLength, fOut, true),
838 f1(f1Tmp),
839 f2(f2Tmp)
840 {
841 /* Verifica di consistenza dei dati iniziali */
842 ASSERT(pNode1 != 0);
843 ASSERT(dynamic_cast<const StructNode *>(pNode1) != 0);
844 ASSERT(pNode1->GetNodeType() == Node::STRUCTURAL);
845 ASSERT(pNode2 != 0);
846 ASSERT(dynamic_cast<const StructNode *>(pNode2) != 0);
847 ASSERT(pNode2->GetNodeType() == Node::STRUCTURAL);
848
849 v = pNode2->GetXCurr() + dynamic_cast<const StructNode *>(pNode2)->GetRCurr()*f2Tmp
850 - pNode1->GetXCurr() - dynamic_cast<const StructNode *>(pNode1)->GetRCurr()*f1Tmp;
851
852 doublereal dDot = v.Dot();
853 if (dDot <= std::numeric_limits<doublereal>::epsilon()) {
854 silent_cerr("RodWithOffset(" << GetLabel() << "): "
855 "inital length must be non-null" << std::endl);
856 throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
857 }
858
859 dElle = sqrt(dDot);
860
861 ASSERT(dLength > std::numeric_limits<doublereal>::epsilon());
862 }
863
864 /* Distruttore */
~RodWithOffset(void)865 RodWithOffset::~RodWithOffset(void)
866 {
867 NO_OP;
868 }
869
870 /* Contributo al file di restart */
871 std::ostream&
Restart(std::ostream & out) const872 RodWithOffset::Restart(std::ostream& out) const
873 {
874 Joint::Restart(out) << ", rod, "
875 << pNode1->GetLabel() << ", "
876 "position, reference, node, ", f1.Write(out, ", ") << ", "
877 << pNode2->GetLabel() << ", "
878 "position, reference, node, ", f2.Write(out, ", ") << ", "
879 << dL0;
880 return pGetConstLaw()->Restart(out) << ';' << std::endl;
881 }
882
883 void
AfterConvergence(const VectorHandler & X,const VectorHandler & XP)884 RodWithOffset::AfterConvergence(const VectorHandler& X,
885 const VectorHandler& XP)
886 {
887 ConstitutiveLaw1DOwner::AfterConvergence(dEpsilon, dEpsilonPrime);
888 }
889
890 VariableSubMatrixHandler&
AssJac(VariableSubMatrixHandler & WorkMat,doublereal dCoef,const VectorHandler &,const VectorHandler &)891 RodWithOffset::AssJac(VariableSubMatrixHandler& WorkMat,
892 doublereal dCoef,
893 const VectorHandler& /* XCurr */ ,
894 const VectorHandler& /* XPrimeCurr */ )
895 {
896 DEBUGCOUT("Entering RodWithOffset::AssJac()" << std::endl);
897
898 FullSubMatrixHandler& WM = WorkMat.SetFull();
899
900 /* Dimensiona e resetta la matrice di lavoro */
901 integer iNumRows = 0;
902 integer iNumCols = 0;
903 WorkSpaceDim(&iNumRows, &iNumCols);
904 WM.ResizeReset(iNumRows, iNumCols);
905
906 /* Recupera gli indici */
907 integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
908 integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
909 integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
910 integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
911
912 /* Setta gli indici della matrice */
913 for (int iCnt = 1; iCnt <= 6; iCnt++) {
914 WM.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
915 WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
916 WM.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
917 WM.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
918 }
919
920 const Mat3x3& R1(dynamic_cast<const StructNode *>(pNode1)->GetRRef());
921 const Mat3x3& R2(dynamic_cast<const StructNode *>(pNode2)->GetRRef());
922 Vec3 f1Tmp(R1*f1);
923 Vec3 f2Tmp(R2*f2);
924
925 const Vec3& v1(pNode1->GetVCurr());
926 const Vec3& v2(pNode2->GetVCurr());
927 const Vec3& Omega1(dynamic_cast<const StructNode *>(pNode1)->GetWRef());
928 const Vec3& Omega2(dynamic_cast<const StructNode *>(pNode2)->GetWRef());
929
930 /* Velocita' di deformazione */
931 Vec3 vPrime(v2 + Omega2.Cross(f2Tmp) - v1 - Omega1.Cross(f1Tmp));
932
933 /* Forza e slopes */
934 doublereal dF = GetF();
935 doublereal dFDE = GetFDE();
936 doublereal dFDEPrime = GetFDEPrime();
937
938 /* Vettore forza */
939 Vec3 F = v*(dF/dElle);
940
941 Mat3x3 K(v.Tens(v*(dCoef*(dFDE/dL0 - (dEpsilonPrime*dFDEPrime + dF)/dElle)/(dElle*dElle))));
942 if (dFDEPrime != 0.) {
943 K += v.Tens(vPrime*(dCoef*dFDEPrime/(dElle*dElle*dL0)));
944 }
945 doublereal d = dCoef*dF/dElle;
946 for (unsigned iCnt = 1; iCnt <= 3; iCnt++) {
947 K(iCnt, iCnt) += d;
948 }
949
950 Mat3x3 KPrime;
951 if (dFDEPrime != 0.) {
952 KPrime = v.Tens(v*((dFDEPrime)/(dL0*dElle*dElle)));
953 }
954
955 /* Termini di forza diagonali */
956 Mat3x3 Tmp1(K);
957 if (dFDEPrime != 0.) {
958 Tmp1 += KPrime;
959 }
960 WM.Add(1, 1, Tmp1);
961 WM.Add(6 + 1, 6 + 1, Tmp1);
962
963 /* Termini di coppia, nodo 1 */
964 Mat3x3 Tmp2 = f1Tmp.Cross(Tmp1);
965 WM.Add(3 + 1, 1, Tmp2);
966 WM.Sub(3 + 1, 6 + 1, Tmp2);
967
968 /* Termini di coppia, nodo 2 */
969 Tmp2 = f2Tmp.Cross(Tmp1);
970 WM.Add(9 + 1, 6 + 1, Tmp2);
971 WM.Sub(9 + 1, 1, Tmp2);
972
973 /* termini di forza extradiagonali */
974 WM.Sub(1, 6 + 1, Tmp1);
975 WM.Sub(6 + 1, 1, Tmp1);
976
977 /* Termini di rotazione, Delta g1 */
978 Mat3x3 Tmp3 = Tmp1*Mat3x3(MatCross, -f1Tmp);
979 if (dFDEPrime != 0.) {
980 Tmp3 += KPrime*Mat3x3(MatCross, f1Tmp.Cross(Omega1*dCoef));
981 }
982 WM.Add(1, 3 + 1, Tmp3);
983 WM.Sub(6 + 1, 3 + 1, Tmp3);
984
985 /* Termini di coppia, Delta g1 */
986 Tmp2 = f1Tmp.Cross(Tmp3) + Mat3x3(MatCrossCross, F, f1Tmp*dCoef);
987 WM.Add(3 + 1, 3 + 1, Tmp2);
988 Tmp2 = f2Tmp.Cross(Tmp3);
989 WM.Sub(9 + 1, 3 + 1, Tmp2);
990
991 /* Termini di rotazione, Delta g2 */
992 Tmp3 = Tmp1*Mat3x3(MatCross, -f2Tmp);
993 if (dFDEPrime != 0.) {
994 Tmp3 += KPrime*Mat3x3(MatCross, f2Tmp.Cross(Omega2*dCoef));
995 }
996 WM.Add(6 + 1, 9 + 1, Tmp3);
997 WM.Sub(1, 9 + 1, Tmp3);
998
999 /* Termini di coppia, Delta g2 */
1000 Tmp2 = f2Tmp.Cross(Tmp3) + Mat3x3(MatCrossCross, F, f2Tmp*dCoef);
1001 WM.Add(9 + 1, 9 + 1, Tmp2);
1002 Tmp2 = f1Tmp.Cross(Tmp3);
1003 WM.Sub(3 + 1, 9 + 1, Tmp2);
1004
1005 return WorkMat;
1006 }
1007
1008 SubVectorHandler&
AssRes(SubVectorHandler & WorkVec,doublereal,const VectorHandler &,const VectorHandler &)1009 RodWithOffset::AssRes(SubVectorHandler& WorkVec,
1010 doublereal /* dCoef */ ,
1011 const VectorHandler& /* XCurr */ ,
1012 const VectorHandler& /* XPrimeCurr */ )
1013 {
1014 DEBUGCOUT("RodWithOffset::AssRes()" << std::endl);
1015
1016 /* Dimensiona e resetta la matrice di lavoro */
1017 integer iNumRows = 0;
1018 integer iNumCols = 0;
1019 WorkSpaceDim(&iNumRows, &iNumCols);
1020 WorkVec.ResizeReset(iNumRows);
1021
1022 /* Indici */
1023 integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
1024 integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
1025
1026 /* Setta gli indici */
1027 for (int iCnt = 1; iCnt <= 6; iCnt++) {
1028 WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
1029 WorkVec.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
1030 }
1031
1032 AssVec(WorkVec);
1033
1034 return WorkVec;
1035 }
1036
1037 void
AssVec(SubVectorHandler & WorkVec)1038 RodWithOffset::AssVec(SubVectorHandler& WorkVec)
1039 {
1040 DEBUGCOUT("RodWithOffset::AssVec()" << std::endl);
1041
1042 /* Dati */
1043 const Mat3x3& R1(dynamic_cast<const StructNode *>(pNode1)->GetRCurr());
1044 const Mat3x3& R2(dynamic_cast<const StructNode *>(pNode2)->GetRCurr());
1045 Vec3 f1Tmp(R1*f1);
1046 Vec3 f2Tmp(R2*f2);
1047 const Vec3& x1(pNode1->GetXCurr());
1048 const Vec3& x2(pNode2->GetXCurr());
1049
1050 const Vec3& v1(pNode1->GetVCurr());
1051 const Vec3& v2(pNode2->GetVCurr());
1052 const Vec3& Omega1(dynamic_cast<const StructNode *>(pNode1)->GetWCurr());
1053 const Vec3& Omega2(dynamic_cast<const StructNode *>(pNode2)->GetWCurr());
1054
1055 /* v = x2-x1 */
1056 v = x2 + f2Tmp - x1 - f1Tmp;
1057 doublereal dCross = v.Dot();
1058
1059 /* Verifica che la distanza non sia nulla */
1060 if (dCross <= std::numeric_limits<doublereal>::epsilon()) {
1061 silent_cerr("RodWithOffset(" << GetLabel() << "): "
1062 "null distance between nodes " << pNode1->GetLabel()
1063 << " and " << pNode2->GetLabel() << std::endl);
1064 throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
1065 }
1066
1067 /* Lunghezza corrente */
1068 dElle = sqrt(dCross);
1069
1070 /* Deformazione */
1071 dEpsilon = dCalcEpsilon();
1072
1073 /* Velocita' di deformazione */
1074 Vec3 vPrime(v2 + Omega2.Cross(f2Tmp) - v1 - Omega1.Cross(f1Tmp));
1075 dEpsilonPrime = (v.Dot(vPrime))/(dElle*dL0);
1076
1077 /* Ampiezza della forza */
1078 bool ChangeJac(false);
1079 try {
1080 ConstitutiveLaw1DOwner::Update(dEpsilon, dEpsilonPrime);
1081
1082 } catch (Elem::ChangedEquationStructure) {
1083 ChangeJac = true;
1084 }
1085
1086 doublereal dF = GetF();
1087
1088 /* Vettore forza */
1089 Vec3 F(v*(dF/dElle));
1090
1091 WorkVec.Add(1, F);
1092 WorkVec.Add(3 + 1, f1Tmp.Cross(F));
1093 WorkVec.Sub(6 + 1, F);
1094 WorkVec.Sub(9 + 1, f2Tmp.Cross(F));
1095
1096 if (ChangeJac) {
1097 throw Elem::ChangedEquationStructure(MBDYN_EXCEPT_ARGS);
1098 }
1099 }
1100
1101 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
1102 VariableSubMatrixHandler&
InitialAssJac(VariableSubMatrixHandler & WorkMat,const VectorHandler &)1103 RodWithOffset::InitialAssJac(VariableSubMatrixHandler& WorkMat,
1104 const VectorHandler& /* XCurr */ )
1105 {
1106 DEBUGCOUT("Entering RodWithOffset::InitialAssJac()" << std::endl);
1107
1108 FullSubMatrixHandler& WM = WorkMat.SetFull();
1109
1110 /* Dimensiona e resetta la matrice di lavoro */
1111 integer iNumRows = 0;
1112 integer iNumCols = 0;
1113 InitialWorkSpaceDim(&iNumRows, &iNumCols);
1114 WM.ResizeReset(iNumRows, iNumCols);
1115
1116 /* Recupera gli indici */
1117 integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
1118 integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
1119 integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
1120 integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
1121
1122 /* Setta gli indici della matrice */
1123 for (int iCnt = 1; iCnt <= 6; iCnt++) {
1124 WM.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
1125 WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
1126 WM.PutColIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
1127
1128 WM.PutRowIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1129 WM.PutColIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
1130 WM.PutColIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
1131 }
1132
1133 const Mat3x3& R1(dynamic_cast<const StructNode *>(pNode1)->GetRRef());
1134 const Mat3x3& R2(dynamic_cast<const StructNode *>(pNode2)->GetRRef());
1135 Vec3 f1Tmp(R1*f1);
1136 Vec3 f2Tmp(R2*f2);
1137
1138 const Vec3& v1(pNode1->GetVCurr());
1139 const Vec3& v2(pNode2->GetVCurr());
1140 const Vec3& Omega1(dynamic_cast<const StructNode *>(pNode1)->GetWRef());
1141 const Vec3& Omega2(dynamic_cast<const StructNode *>(pNode2)->GetWRef());
1142
1143 /* Velocita' di deformazione */
1144 Vec3 vPrime(v2 + Omega2.Cross(f2Tmp) - v1 - Omega1.Cross(f1Tmp));
1145
1146 /* Forza e slopes */
1147 doublereal dF = GetF();
1148 doublereal dFDE = GetFDE();
1149 doublereal dFDEPrime = GetFDEPrime();
1150
1151 /* Vettore forza */
1152 Vec3 F = v*(dF/dElle);
1153
1154 Mat3x3 K(v.Tens(v*((dFDE/dL0 - (dEpsilonPrime*dFDEPrime + dF)/dElle)/(dElle*dElle))));
1155 if (dFDEPrime != 0.) {
1156 K += v.Tens(vPrime*(dFDEPrime/(dElle*dElle*dL0)));
1157 }
1158 doublereal d = dF/dElle;
1159 for (unsigned iCnt = 1; iCnt <= 3; iCnt++) {
1160 K(iCnt, iCnt) += d;
1161 }
1162
1163 Mat3x3 KPrime;
1164 if (dFDEPrime != 0.) {
1165 KPrime = v.Tens(v*((dFDEPrime)/(dL0*dElle*dElle)));
1166 }
1167
1168 /* Termini di forza diagonali */
1169 WM.Add(1, 1, K);
1170 WM.Add(6 + 1, 12 + 1, K);
1171
1172 /* Termini di coppia, nodo 1 */
1173 Mat3x3 Tmp2 = f1Tmp.Cross(K);
1174 WM.Add(3 + 1, 1, Tmp2);
1175 WM.Sub(3 + 1, 12 + 1, Tmp2);
1176
1177 /* Termini di coppia, nodo 2 */
1178 Tmp2 = f2Tmp.Cross(K);
1179 WM.Add(9 + 1, 12 + 1, Tmp2);
1180 WM.Sub(9 + 1, 1, Tmp2);
1181
1182 /* termini di forza extradiagonali */
1183 WM.Sub(1, 12 + 1, K);
1184 WM.Sub(6 + 1, 1, K);
1185
1186 if (dFDEPrime != 0.) {
1187 /* Termini di forza diagonali */
1188 WM.Add(1, 6 + 1, KPrime);
1189 WM.Add(6 + 1, 18 + 1, KPrime);
1190
1191 /* Termini di coppia, nodo 1 */
1192 Tmp2 = f1Tmp.Cross(KPrime);
1193 WM.Add(3 + 1, 6 + 1, Tmp2);
1194 WM.Sub(3 + 1, 18 + 1, Tmp2);
1195
1196 /* Termini di coppia, nodo 2 */
1197 Tmp2 = f2Tmp.Cross(KPrime);
1198 WM.Add(9 + 1, 18 + 1, Tmp2);
1199 WM.Sub(9 + 1, 6 + 1, Tmp2);
1200
1201 /* termini di forza extradiagonali */
1202 WM.Sub(1, 18 + 1, KPrime);
1203 WM.Sub(6 + 1, 6 + 1, KPrime);
1204 }
1205
1206 /* Termini di rotazione, Delta g1 */
1207 Mat3x3 Tmp3 = K*Mat3x3(MatCross, -f1Tmp);
1208 if (dFDEPrime != 0.) {
1209 Tmp3 -= KPrime*Mat3x3(MatCrossCross, Omega1, f1Tmp);
1210 }
1211 WM.Add(1, 3 + 1, Tmp3);
1212 WM.Sub(6 + 1, 3 + 1, Tmp3);
1213
1214 /* Termini di coppia, Delta g1 */
1215 Tmp2 = f1Tmp.Cross(Tmp3) + Mat3x3(MatCrossCross, F, f1Tmp);
1216 WM.Add(3 + 1, 3 + 1, Tmp2);
1217 Tmp2 = f2Tmp.Cross(Tmp3);
1218 WM.Sub(9 + 1, 3 + 1, Tmp2);
1219
1220 /* Termini di rotazione, Delta g2 */
1221 Tmp3 = K*Mat3x3(MatCross, -f2Tmp);
1222 if (dFDEPrime != 0.) {
1223 Tmp3 -= KPrime*Mat3x3(MatCrossCross, Omega2, f2Tmp);
1224 }
1225 WM.Add(6 + 1, 15 + 1, Tmp3);
1226 WM.Sub(1, 15 + 1, Tmp3);
1227
1228 /* Termini di coppia, Delta g2 */
1229 Tmp2 = f2Tmp.Cross(Tmp3) + Mat3x3(MatCrossCross, F, f2Tmp);
1230 WM.Add(9 + 1, 15 + 1, Tmp2);
1231 Tmp2 = f1Tmp.Cross(Tmp3);
1232 WM.Sub(3 + 1, 15 + 1, Tmp2);
1233
1234 if (dFDEPrime != 0.) {
1235 /* Termini di rotazione, Delta w1 */
1236 Tmp3 = KPrime*Mat3x3(MatCross, -f1Tmp);
1237 WM.Add(1, 9 + 1, Tmp3);
1238 WM.Sub(6 + 1, 9 + 1, Tmp3);
1239
1240 /* Termini di coppia, Delta w1 */
1241 Tmp2 = f1Tmp.Cross(Tmp3);
1242 WM.Add(3 + 1, 9 + 1, Tmp2);
1243 Tmp2 = f2Tmp.Cross(Tmp3);
1244 WM.Sub(9 + 1, 9 + 1, Tmp2);
1245
1246 /* Termini di rotazione, Delta w2 */
1247 Tmp3 = KPrime*Mat3x3(MatCross, -f2Tmp);
1248 WM.Add(6 + 1, 21 + 1, Tmp3);
1249 WM.Sub(1, 21 + 1, Tmp3);
1250
1251 /* Termini di coppia, Delta w2 */
1252 Tmp2 = f2Tmp.Cross(Tmp3);
1253 WM.Add(9 + 1, 21 + 1, Tmp2);
1254 Tmp2 = f1Tmp.Cross(Tmp3);
1255 WM.Sub(3 + 1, 21 + 1, Tmp2);
1256 }
1257
1258 return WorkMat;
1259 }
1260
1261 /* Contributo al residuo durante l'assemblaggio iniziale */
1262 SubVectorHandler&
InitialAssRes(SubVectorHandler & WorkVec,const VectorHandler &)1263 RodWithOffset::InitialAssRes(SubVectorHandler& WorkVec,
1264 const VectorHandler& /* XCurr */ )
1265 {
1266 DEBUGCOUT("RodWithOffset::InitialAssRes()" << std::endl);
1267
1268 /* Dimensiona e resetta la matrice di lavoro */
1269 integer iNumRows = 0;
1270 integer iNumCols = 0;
1271 InitialWorkSpaceDim(&iNumRows, &iNumCols);
1272 WorkVec.ResizeReset(iNumRows);
1273
1274 /* Indici */
1275 integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
1276 integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
1277
1278 /* Setta gli indici */
1279 for (int iCnt = 1; iCnt <= 6; iCnt++) {
1280 WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
1281 WorkVec.PutRowIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
1282 }
1283
1284 AssVec(WorkVec);
1285
1286 return WorkVec;
1287 }
1288
1289 /* RodWithOffset - end */
1290