1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/spherj.cc,v 1.47 2017/01/12 14:46:44 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 /* Giunti sferici */
33
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35
36 #include "spherj.h"
37 #include "Rot.hh"
38
39
40 /* SphericalHingeJoint - begin */
41
42 /* Costruttore non banale */
SphericalHingeJoint(unsigned int uL,const DofOwner * pDO,const StructNode * pN1,const StructNode * pN2,const Vec3 & dTmp1,const Mat3x3 & RTmp1h,const Vec3 & dTmp2,const Mat3x3 & RTmp2h,const OrientationDescription & od,flag fOut)43 SphericalHingeJoint::SphericalHingeJoint(unsigned int uL, const DofOwner* pDO,
44 const StructNode* pN1,
45 const StructNode* pN2,
46 const Vec3& dTmp1, const Mat3x3& RTmp1h,
47 const Vec3& dTmp2, const Mat3x3& RTmp2h,
48 const OrientationDescription& od,
49 flag fOut)
50 : Elem(uL, fOut),
51 Joint(uL, pDO, fOut),
52 pNode1(pN1), pNode2(pN2),
53 #ifdef USE_NETCDF
54 Var_Phi(0),
55 #endif // USE_NETCDF
56 d1(dTmp1), R1h(RTmp1h),
57 d2(dTmp2), R2h(RTmp2h),
58 F(Zero3),
59 od(od)
60 {
61 NO_OP;
62 }
63
64
65 /* Distruttore banale */
~SphericalHingeJoint(void)66 SphericalHingeJoint::~SphericalHingeJoint(void)
67 {
68 NO_OP;
69 };
70
71
72 /* Contributo al file di restart */
Restart(std::ostream & out) const73 std::ostream& SphericalHingeJoint::Restart(std::ostream& out) const
74 {
75 Joint::Restart(out) << ", spherical hinge, "
76 << pNode1->GetLabel() << ", reference, node, ",
77 d1.Write(out, ", ") << ", hinge, reference, node, 1, ", (R1h.GetVec(1)).Write(out, ", ")
78 << ", 2, ", (R1h.GetVec(2)).Write(out, ", ") << ", "
79 << pNode2->GetLabel() << ", reference, node, ",
80 d2.Write(out, ", ") << ", hinge, reference, node, 1, ", (R2h.GetVec(1)).Write(out, ", ")
81 << ", 2, ", (R2h.GetVec(2)).Write(out, ", ") << ';' << std::endl;
82
83 return out;
84 }
85
86
87 /* Assemblaggio jacobiano */
88 VariableSubMatrixHandler&
AssJac(VariableSubMatrixHandler & WorkMat,doublereal dCoef,const VectorHandler &,const VectorHandler &)89 SphericalHingeJoint::AssJac(VariableSubMatrixHandler& WorkMat,
90 doublereal dCoef,
91 const VectorHandler& /* XCurr */ ,
92 const VectorHandler& /* XPrimeCurr */ )
93 {
94 DEBUGCOUT("Entering SphericalHingeJoint::AssJac()" << std::endl);
95
96 SparseSubMatrixHandler& WM = WorkMat.SetSparse();
97 WM.ResizeReset(54, 1);
98
99 integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
100 integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
101 integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
102 integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
103 integer iFirstReactionIndex = iGetFirstIndex();
104
105 Vec3 dTmp1(pNode1->GetRRef()*d1);
106 Vec3 dTmp2(pNode2->GetRRef()*d2);
107
108
109 /*
110 * L'equazione di vincolo afferma che il punto in cui si trova la
111 * cerniera deve essere consistente con la posizione dei due nodi:
112 * x2 + d2 = x1 + d1
113 *
114 * con: d2 = R2 * d2_0
115 * d1 = R1 * d1_0
116 *
117 * La forza e' data dalla reazione vincolare F, nel sistema globale
118 * La coppia dovuta all'eccentricita' e' data rispettivamente da:
119 * -d1 /\ F per il nodo 1,
120 * d2 /\ F per il nodo 2
121 *
122 *
123 * x1 g1 x2 g2 F
124 * Q1 | 0 0 0 0 I | | x1 | | -F |
125 * G1 | 0 cF/\d1/\ 0 0 d1/\ | | g1 | | -d1/\F |
126 * Q2 | 0 0 0 -cF/\d2/\ -I | | x2 | = | F |
127 * G2 | 0 0 0 0 -d2/\ | | g2 | | d2/\F |
128 * F | -c*I c*d1/\ c*I -c*d2/\ 0 | | F | | x1+d1-x2-d2 |
129 *
130 * con d1 = R1*d01, d2 = R2*d02, c = dCoef
131 */
132
133 /* Moltiplico la forza per il coefficiente del metodo.
134 * Nota: F, la reazione vincolare, e' stata aggiornata da AssRes */
135 Vec3 FTmp = F*dCoef;
136
137 /* termini di reazione sul nodo 1 */
138 WM.PutDiag(1, iNode1FirstMomIndex, iFirstReactionIndex, 1.);
139 WM.PutCross(4, iNode1FirstMomIndex+3, iFirstReactionIndex, dTmp1);
140
141 WM.PutMat3x3(10, iNode1FirstMomIndex+3,
142 iNode1FirstPosIndex+3, Mat3x3(MatCrossCross, FTmp, dTmp1));
143
144 /* termini di reazione sul nodo 2 */
145 WM.PutDiag(19, iNode2FirstMomIndex, iFirstReactionIndex, -1.);
146 WM.PutCross(22, iNode2FirstMomIndex+3, iFirstReactionIndex, -dTmp2);
147
148 WM.PutMat3x3(28, iNode2FirstMomIndex+3,
149 iNode2FirstPosIndex+3, Mat3x3(MatCrossCross, FTmp, -dTmp2));
150
151 /* Modifica: divido le equazioni di vincolo per dCoef */
152
153 /* termini di vincolo dovuti al nodo 1 */
154 WM.PutDiag(37, iFirstReactionIndex, iNode1FirstPosIndex, -1.);
155 WM.PutCross(40, iFirstReactionIndex, iNode1FirstPosIndex+3, dTmp1);
156
157 /* termini di vincolo dovuti al nodo 1 */
158 WM.PutDiag(46, iFirstReactionIndex, iNode2FirstPosIndex, 1.);
159 WM.PutCross(49, iFirstReactionIndex, iNode2FirstPosIndex+3, -dTmp2);
160
161 return WorkMat;
162 }
163
164
165 /* Assemblaggio residuo */
AssRes(SubVectorHandler & WorkVec,doublereal dCoef,const VectorHandler & XCurr,const VectorHandler &)166 SubVectorHandler& SphericalHingeJoint::AssRes(SubVectorHandler& WorkVec,
167 doublereal dCoef,
168 const VectorHandler& XCurr,
169 const VectorHandler& /* XPrimeCurr */ )
170 {
171 DEBUGCOUT("Entering SphericalHingeJoint::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 iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
180 integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
181 integer iFirstReactionIndex = iGetFirstIndex();
182
183 /* Indici dei nodi */
184 for (int iCnt = 1; iCnt <= 6; iCnt++) {
185 WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex+iCnt);
186 WorkVec.PutRowIndex(6+iCnt, iNode2FirstMomIndex+iCnt);
187 }
188
189 /* Indici del vincolo */
190 for (int iCnt = 1; iCnt <= 3; iCnt++) {
191 WorkVec.PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
192 }
193
194 F = Vec3(XCurr, iFirstReactionIndex+1);
195
196 const Vec3& x1(pNode1->GetXCurr());
197 const Vec3& x2(pNode2->GetXCurr());
198
199 Vec3 dTmp1(pNode1->GetRCurr()*d1);
200 Vec3 dTmp2(pNode2->GetRCurr()*d2);
201
202 WorkVec.Sub(1, F);
203 WorkVec.Sub(4, dTmp1.Cross(F));
204 WorkVec.Add(7, F);
205 WorkVec.Add(10, dTmp2.Cross(F));
206
207 /* Modifica: divido le equazioni di vincolo per dCoef */
208 ASSERT(dCoef != 0.);
209 WorkVec.Add(13, (x1+dTmp1-x2-dTmp2)/dCoef);
210
211 return WorkVec;
212 }
213
214
GetEqType(unsigned int i) const215 DofOrder::Order SphericalHingeJoint::GetEqType(unsigned int i) const {
216 ASSERTMSGBREAK(i >=0 and i < iGetNumDof(),
217 "INDEX ERROR in SphericalHingeJoint::GetEqType");
218 return DofOrder::ALGEBRAIC;
219 }
220
221 void
OutputPrepare(OutputHandler & OH)222 SphericalHingeJoint::OutputPrepare(OutputHandler& OH)
223 {
224 if (bToBeOutput()) {
225 #ifdef USE_NETCDF
226 if (OH.UseNetCDF(OutputHandler::JOINTS)) {
227 std::string name;
228 OutputPrepare_int("spherical hinge", OH, name);
229
230 Var_Phi = OH.CreateRotationVar(name, "", od, "global");
231
232 }
233 #endif // USE_NETCDF
234 }
235 }
236
237 /* Output (da mettere a punto) */
Output(OutputHandler & OH) const238 void SphericalHingeJoint::Output(OutputHandler& OH) const
239 {
240 if (bToBeOutput()) {
241 Mat3x3 R1Tmp(pNode1->GetRCurr()*R1h);
242 Mat3x3 RTmp(R1Tmp.MulTM(pNode2->GetRCurr()*R2h));
243 Vec3 E;
244 switch (od) {
245 case EULER_123:
246 E = MatR2EulerAngles123(RTmp)*dRaDegr;
247 break;
248
249 case EULER_313:
250 E = MatR2EulerAngles313(RTmp)*dRaDegr;
251 break;
252
253 case EULER_321:
254 E = MatR2EulerAngles321(RTmp)*dRaDegr;
255 break;
256
257 case ORIENTATION_VECTOR:
258 E = RotManip::VecRot(RTmp);
259 break;
260
261 case ORIENTATION_MATRIX:
262 break;
263
264 default:
265 /* impossible */
266 break;
267 }
268
269 #ifdef USE_NETCDF
270 if (OH.UseNetCDF(OutputHandler::JOINTS)) {
271 Var_F_local->put_rec((R1Tmp.MulTV(F)).pGetVec(), OH.GetCurrentStep());
272 Var_M_local->put_rec(Zero3.pGetVec(), OH.GetCurrentStep());
273 Var_F_global->put_rec(F.pGetVec(), OH.GetCurrentStep());
274 Var_M_global->put_rec(Zero3.pGetVec(), OH.GetCurrentStep());
275 switch (od) {
276 case EULER_123:
277 case EULER_313:
278 case EULER_321:
279 case ORIENTATION_VECTOR:
280 Var_Phi->put_rec(E.pGetVec(), OH.GetCurrentStep());
281 break;
282
283 case ORIENTATION_MATRIX:
284 Var_Phi->put_rec(RTmp.pGetMat(), OH.GetCurrentStep());
285 break;
286
287 default:
288 /* impossible */
289 break;
290 }
291 }
292 #endif // USE_NETCDF
293 if (OH.UseText(OutputHandler::JOINTS)) {
294 Joint::Output(OH.Joints(), "SphericalHinge", GetLabel(),
295 R1Tmp.MulTV(F), Zero3, F, Zero3)
296 << " ";
297
298 switch (od) {
299 case EULER_123:
300 case EULER_313:
301 case EULER_321:
302 case ORIENTATION_VECTOR:
303 OH.Joints() << E;
304 break;
305
306 case ORIENTATION_MATRIX:
307 OH.Joints() << RTmp;
308 break;
309
310 default:
311 /* impossible */
312 break;
313 }
314
315 OH.Joints() << std::endl;
316 }
317 }
318 }
319
320 void
SetValue(DataManager * pDM,VectorHandler & X,VectorHandler & XP,SimulationEntity::Hints * ph)321 SphericalHingeJoint::SetValue(DataManager *pDM,
322 VectorHandler& X, VectorHandler& XP,
323 SimulationEntity::Hints *ph)
324 {
325 if (ph) {
326 for (unsigned i = 0; i < ph->size(); i++) {
327 Joint::JointHint *pjh = dynamic_cast<Joint::JointHint *>((*ph)[i]);
328
329 if (pjh == 0) {
330 continue;
331 }
332
333 if (dynamic_cast<Joint::OffsetHint<1> *>(pjh)) {
334 const Mat3x3& R1(pNode1->GetRCurr());
335 Vec3 dTmp2(pNode2->GetRCurr()*d2);
336
337 d1 = R1.MulTV(pNode2->GetXCurr() + dTmp2 - pNode1->GetXCurr());
338
339 } else if (dynamic_cast<Joint::OffsetHint<2> *>(pjh)) {
340 const Mat3x3& R2(pNode2->GetRCurr());
341 Vec3 dTmp1(pNode1->GetRCurr()*d1);
342
343 d2 = R2.MulTV(pNode1->GetXCurr() + dTmp1 - pNode2->GetXCurr());
344
345 } else if (dynamic_cast<Joint::ReactionsHint *>(pjh)) {
346 /* TODO */
347 }
348 }
349 }
350 }
351
352 Hint *
ParseHint(DataManager * pDM,const char * s) const353 SphericalHingeJoint::ParseHint(DataManager *pDM, const char *s) const
354 {
355 if (strncasecmp(s, "offset{" /*}*/, STRLENOF("offset{" /*}*/)) == 0) {
356 s += STRLENOF("offset{" /*}*/);
357
358 if (strcmp(&s[1], /*{*/ "}") != 0) {
359 return 0;
360 }
361
362 switch (s[0]) {
363 case '1':
364 return new Joint::OffsetHint<1>;
365
366 case '2':
367 return new Joint::OffsetHint<2>;
368 }
369 }
370
371 return 0;
372 }
373
374 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
375 VariableSubMatrixHandler&
InitialAssJac(VariableSubMatrixHandler & WorkMat,const VectorHandler & XCurr)376 SphericalHingeJoint::InitialAssJac(VariableSubMatrixHandler& WorkMat,
377 const VectorHandler& XCurr)
378 {
379 DEBUGCOUT("Entering SphericalHingeJoint::InitialAssJac()" << std::endl);
380
381 FullSubMatrixHandler& WM = WorkMat.SetFull();
382
383 /* Dimensiona e resetta la matrice di lavoro */
384 integer iNumRows = 0;
385 integer iNumCols = 0;
386 InitialWorkSpaceDim(&iNumRows, &iNumCols);
387 WM.ResizeReset(iNumRows, iNumCols);
388
389 /* Equazioni: vedi joints.dvi */
390
391 /* Indici */
392 integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
393 integer iNode1FirstVelIndex = iNode1FirstPosIndex+6;
394 integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
395 integer iNode2FirstVelIndex = iNode2FirstPosIndex+6;
396 integer iFirstReactionIndex = iGetFirstIndex();
397 integer iReactionPrimeIndex = iFirstReactionIndex+3;
398
399 /* Setto gli indici */
400 for (int iCnt = 1; iCnt <= 6; iCnt++) {
401 WM.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
402 WM.PutColIndex(iCnt, iNode1FirstPosIndex+iCnt);
403 WM.PutRowIndex(6+iCnt, iNode1FirstVelIndex+iCnt);
404 WM.PutColIndex(6+iCnt, iNode1FirstVelIndex+iCnt);
405 WM.PutRowIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
406 WM.PutColIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
407 WM.PutRowIndex(18+iCnt, iNode2FirstVelIndex+iCnt);
408 WM.PutColIndex(18+iCnt, iNode2FirstVelIndex+iCnt);
409 WM.PutRowIndex(24+iCnt, iFirstReactionIndex+iCnt);
410 WM.PutColIndex(24+iCnt, iFirstReactionIndex+iCnt);
411 }
412
413 /* Matrici identita' */
414
415 for (int iCnt = 1; iCnt <= 3; iCnt++) {
416 /* Contributo di forza all'equazione della forza, nodo 1 */
417 WM.PutCoef(iCnt, 24+iCnt, 1.);
418
419 /* Contrib. di der. di forza all'eq. della der. della forza, nodo 1 */
420 WM.PutCoef(6+iCnt, 27+iCnt, 1.);
421
422 /* Contributo di forza all'equazione della forza, nodo 2 */
423 WM.PutCoef(12+iCnt, 24+iCnt, -1.);
424
425 /* Contrib. di der. di forza all'eq. della der. della forza, nodo 2 */
426 WM.PutCoef(18+iCnt, 27+iCnt, -1.);
427
428 /* Equazione di vincolo, nodo 1 */
429 WM.PutCoef(24+iCnt, iCnt, -1.);
430
431 /* Derivata dell'equazione di vincolo, nodo 1 */
432 WM.PutCoef(27+iCnt, 6+iCnt, -1.);
433
434 /* Equazione di vincolo, nodo 2 */
435 WM.PutCoef(24+iCnt, 12+iCnt, 1.);
436
437 /* Derivata dell'equazione di vincolo, nodo 2 */
438 WM.PutCoef(27+iCnt, 18+iCnt, 1.);
439 }
440
441 /* Recupera i dati */
442 const Mat3x3& R1(pNode1->GetRRef());
443 const Mat3x3& R2(pNode2->GetRRef());
444 const Vec3& Omega1(pNode1->GetWRef());
445 const Vec3& Omega2(pNode2->GetWRef());
446 /* F e' stata aggiornata da InitialAssRes */
447 Vec3 FPrime(XCurr, iReactionPrimeIndex+1);
448
449 /* Distanza nel sistema globale */
450 Vec3 d1Tmp(R1*d1);
451 Vec3 d2Tmp(R2*d2);
452
453 /* Matrici F/\d1/\, -F/\d2/\ */
454 Mat3x3 FWedged1Wedge(MatCrossCross, F, d1Tmp);
455 Mat3x3 FWedged2Wedge(MatCrossCross, F, -d2Tmp);
456
457 /* Matrici (omega1/\d1)/\, -(omega2/\d2)/\ */
458 Mat3x3 O1Wedged1Wedge(MatCross, Omega1.Cross(d1Tmp));
459 Mat3x3 O2Wedged2Wedge(MatCross, d2Tmp.Cross(Omega2));
460
461 /* Equazione di momento, nodo 1 */
462 WM.Add(4, 4, FWedged1Wedge);
463 WM.Add(4, 25, Mat3x3(MatCross, d1Tmp));
464
465 /* Equazione di momento, nodo 2 */
466 WM.Add(16, 16, FWedged2Wedge);
467 WM.Sub(16, 25, Mat3x3(MatCross, d2Tmp));
468
469 /* Derivata dell'equazione di momento, nodo 1 */
470 WM.Add(10, 4, (Mat3x3(MatCross, FPrime) + Mat3x3(MatCrossCross, F, Omega1))*Mat3x3(MatCross, d1Tmp));
471 WM.Add(10, 10, FWedged1Wedge);
472 WM.Add(10, 25, O1Wedged1Wedge);
473 WM.Add(10, 28, Mat3x3(MatCross, d1Tmp));
474
475 /* Derivata dell'equazione di momento, nodo 2 */
476 WM.Sub(22, 16, (Mat3x3(MatCross, FPrime) + Mat3x3(MatCrossCross, F, Omega2))*Mat3x3(MatCross, d2Tmp));
477 WM.Add(22, 22, FWedged2Wedge);
478 WM.Add(22, 25, O2Wedged2Wedge);
479 WM.Sub(22, 28, Mat3x3(MatCross, d2Tmp));
480
481 /* Equazione di vincolo */
482 WM.Add(25, 4, Mat3x3(MatCross, d1Tmp));
483 WM.Sub(25, 16, Mat3x3(MatCross, d2Tmp));
484
485 /* Derivata dell'equazione di vincolo */
486 WM.Add(28, 4, O1Wedged1Wedge);
487 WM.Add(28, 10, Mat3x3(MatCross, d1Tmp));
488 WM.Add(28, 16, O2Wedged2Wedge);
489 WM.Sub(28, 22, Mat3x3(MatCross, d2Tmp));
490
491 return WorkMat;
492 }
493
494
495 /* Contributo al residuo durante l'assemblaggio iniziale */
496 SubVectorHandler&
InitialAssRes(SubVectorHandler & WorkVec,const VectorHandler & XCurr)497 SphericalHingeJoint::InitialAssRes(SubVectorHandler& WorkVec,
498 const VectorHandler& XCurr)
499 {
500 DEBUGCOUT("Entering SphericalHingeJoint::InitialAssRes()" << std::endl);
501
502 /* Dimensiona e resetta la matrice di lavoro */
503 integer iNumRows = 0;
504 integer iNumCols = 0;
505 InitialWorkSpaceDim(&iNumRows, &iNumCols);
506 WorkVec.ResizeReset(iNumRows);
507
508 /* Indici */
509 integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
510 integer iNode1FirstVelIndex = iNode1FirstPosIndex+6;
511 integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
512 integer iNode2FirstVelIndex = iNode2FirstPosIndex+6;
513 integer iFirstReactionIndex = iGetFirstIndex();
514 integer iReactionPrimeIndex = iFirstReactionIndex+3;
515
516 /* Setta gli indici */
517 for (int iCnt = 1; iCnt <= 6; iCnt++) {
518 WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
519 WorkVec.PutRowIndex(6+iCnt, iNode1FirstVelIndex+iCnt);
520 WorkVec.PutRowIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
521 WorkVec.PutRowIndex(18+iCnt, iNode2FirstVelIndex+iCnt);
522 WorkVec.PutRowIndex(24+iCnt, iFirstReactionIndex+iCnt);
523 }
524
525 /* Recupera i dati */
526 const Vec3& x1(pNode1->GetXCurr());
527 const Vec3& x2(pNode2->GetXCurr());
528 const Vec3& v1(pNode1->GetVCurr());
529 const Vec3& v2(pNode2->GetVCurr());
530 const Mat3x3& R1(pNode1->GetRCurr());
531 const Mat3x3& R2(pNode2->GetRCurr());
532 const Vec3& Omega1(pNode1->GetWCurr());
533 const Vec3& Omega2(pNode2->GetWCurr());
534 F = Vec3(XCurr, iFirstReactionIndex+1);
535 Vec3 FPrime(XCurr, iReactionPrimeIndex+1);
536
537 /* Distanza nel sistema globale */
538 Vec3 d1Tmp(R1*d1);
539 Vec3 d2Tmp(R2*d2);
540
541 /* Vettori omega1/\d1, -omega2/\d2 */
542 Vec3 O1Wedged1(Omega1.Cross(d1Tmp));
543 Vec3 O2Wedged2(Omega2.Cross(d2Tmp));
544
545 /* Equazioni di equilibrio, nodo 1 */
546 WorkVec.Sub(1, F);
547 WorkVec.Add(4, F.Cross(d1Tmp)); /* Sfrutto il fatto che F/\d = -d/\F */
548
549 /* Derivate delle equazioni di equilibrio, nodo 1 */
550 WorkVec.Sub(7, FPrime);
551 WorkVec.Add(10, FPrime.Cross(d1Tmp)-O1Wedged1.Cross(F));
552
553 /* Equazioni di equilibrio, nodo 2 */
554 WorkVec.Add(13, F);
555 WorkVec.Add(16, d2Tmp.Cross(F));
556
557 /* Derivate delle equazioni di equilibrio, nodo 2 */
558 WorkVec.Add(19, FPrime);
559 WorkVec.Add(22, d2Tmp.Cross(FPrime)+O2Wedged2.Cross(F));
560
561 /* Equazione di vincolo */
562 WorkVec.Add(25, x1+d1Tmp-x2-d2Tmp);
563
564 /* Deivata dell'equazione di vincolo */
565 WorkVec.Add(28, v1+O1Wedged1-v2-O2Wedged2);
566
567 return WorkVec;
568 }
569
570 /* SphericalHingeJoint - end */
571
572
573 /* PinJoint - begin */
574 /* Costruttore non banale */
PinJoint(unsigned int uL,const DofOwner * pDO,const StructNode * pN,const Vec3 & X0Tmp,const Vec3 & dTmp,flag fOut)575 PinJoint::PinJoint(unsigned int uL, const DofOwner* pDO,
576 const StructNode* pN,
577 const Vec3& X0Tmp, const Vec3& dTmp, flag fOut)
578 : Elem(uL, fOut),
579 Joint(uL, pDO, fOut), pNode(pN), X0(X0Tmp), d(dTmp), F(Zero3)
580 {
581 NO_OP;
582 }
583
584
585 /* Distruttore banale */
~PinJoint(void)586 PinJoint::~PinJoint(void)
587 {
588 NO_OP;
589 };
590
591
592 /* Contributo al file di restart */
Restart(std::ostream & out) const593 std::ostream& PinJoint::Restart(std::ostream& out) const
594 {
595 Joint::Restart(out) << ", pin, "
596 << pNode->GetLabel() << ", reference, node, ",
597 d.Write(out, ", ") << ", reference, global, ",
598 X0.Write(out, ", ") << ';' << std::endl;
599
600 return out;
601 }
602
603
604 /* Assemblaggio jacobiano */
605 VariableSubMatrixHandler&
AssJac(VariableSubMatrixHandler & WorkMat,doublereal dCoef,const VectorHandler &,const VectorHandler &)606 PinJoint::AssJac(VariableSubMatrixHandler& WorkMat,
607 doublereal dCoef,
608 const VectorHandler& /* XCurr */ ,
609 const VectorHandler& /* XPrimeCurr */ )
610 {
611 DEBUGCOUT("Entering PinJoint::AssJac()" << std::endl);
612
613 SparseSubMatrixHandler& WM = WorkMat.SetSparse();
614 WM.ResizeReset(27, 0);
615
616 integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
617 integer iFirstMomentumIndex = pNode->iGetFirstMomentumIndex();
618 integer iFirstReactionIndex = iGetFirstIndex();
619
620 const Mat3x3& R(pNode->GetRRef());
621 Vec3 dTmp(R*d);
622
623 /*
624 * L'equazione di vincolo afferma che il punto in cui si trova la
625 * cerniera deve essere fissato:
626 * x + d = x0
627 *
628 * con: d = R * d_0
629 *
630 * La forza e' data dalla reazione vincolare F, nel sistema globale
631 * La coppia dovuta all'eccentricita' e' data rispettivamente da:
632 * d /\ F
633 *
634 *
635 * x g F
636 * Q1 | 0 0 I | | x | | -F |
637 * G1 | 0 cF/\d1/\ d/\ | | g | | -d/\F |
638 * F | I d/\ 0 | | F | | (x+d-x0)/c |
639 *
640 * con d = R*d_0, c = dCoef
641 */
642
643 /* termini di reazione sul nodo */
644 for (int iCnt = 1; iCnt <= 3; iCnt++) {
645 WM.PutItem(iCnt, iFirstMomentumIndex+iCnt,
646 iFirstReactionIndex+iCnt, 1.);
647 }
648 WM.PutCross(4, iFirstMomentumIndex+3,
649 iFirstReactionIndex, dTmp);
650
651 /* Nota: F, la reazione vincolare, e' stata aggiornata da AssRes */
652
653 /* Termini diagonali del tipo: c*F/\d/\Delta_g
654 * nota: la forza e' gia' moltiplicata per dCoef */
655 WM.PutMat3x3(10, iFirstMomentumIndex+3,
656 iFirstPositionIndex+3, Mat3x3(MatCrossCross, F*dCoef, dTmp));
657
658 /* Modifica: divido le equazioni di vincolo per dCoef */
659
660 /* termini di vincolo dovuti al nodo 1 */
661 for (int iCnt = 1; iCnt <= 3; iCnt++) {
662 WM.PutItem(18+iCnt, iFirstReactionIndex+iCnt,
663 iFirstPositionIndex+iCnt, -1.);
664 }
665 WM.PutCross(22, iFirstReactionIndex,
666 iFirstPositionIndex+3, dTmp);
667
668 return WorkMat;
669 }
670
671
672 /* Assemblaggio residuo */
AssRes(SubVectorHandler & WorkVec,doublereal dCoef,const VectorHandler & XCurr,const VectorHandler &)673 SubVectorHandler& PinJoint::AssRes(SubVectorHandler& WorkVec,
674 doublereal dCoef,
675 const VectorHandler& XCurr,
676 const VectorHandler& /* XPrimeCurr */ )
677 {
678 DEBUGCOUT("Entering PinJoint::AssRes()" << std::endl);
679
680 /* Dimensiona e resetta la matrice di lavoro */
681 integer iNumRows = 0;
682 integer iNumCols = 0;
683 WorkSpaceDim(&iNumRows, &iNumCols);
684 WorkVec.ResizeReset(iNumRows);
685
686 integer iFirstMomentumIndex = pNode->iGetFirstMomentumIndex();
687 integer iFirstReactionIndex = iGetFirstIndex();
688
689 /* Indici dei nodi */
690 for (int iCnt = 1; iCnt <= 6; iCnt++) {
691 WorkVec.PutRowIndex(iCnt, iFirstMomentumIndex+iCnt);
692 }
693
694
695 /* Indici del vincolo */
696 for(int iCnt = 1; iCnt <= 3; iCnt++) {
697 WorkVec.PutRowIndex(6+iCnt, iFirstReactionIndex+iCnt);
698 }
699
700 F = Vec3(XCurr, iFirstReactionIndex+1);
701
702 const Vec3& x(pNode->GetXCurr());
703 const Mat3x3& R(pNode->GetRCurr());
704
705 Vec3 dTmp(R*d);
706
707 WorkVec.Sub(1, F);
708 WorkVec.Add(4, F.Cross(dTmp)); /* Sfrutto il fatto che F/\d = -d/\F */
709
710 /* Modifica: divido le equazioni di vincolo per dCoef */
711 ASSERT(dCoef != 0.);
712 WorkVec.Add(7, (x+dTmp-X0)/dCoef);
713
714 return WorkVec;
715 }
716
GetEqType(unsigned int i) const717 DofOrder::Order PinJoint::GetEqType(unsigned int i) const {
718 ASSERTMSGBREAK(i >=0 and i < iGetNumDof(),
719 "INDEX ERROR in PinJoint::GetEqType");
720 return DofOrder::ALGEBRAIC;
721 }
722
723 /* Output (da mettere a punto) */
Output(OutputHandler & OH) const724 void PinJoint::Output(OutputHandler& OH) const
725 {
726 if (bToBeOutput()) {
727 Joint::Output(OH.Joints(), "Pin", GetLabel(), F, Zero3, F, Zero3)
728 << " " << MatR2EulerAngles(pNode->GetRCurr())*dRaDegr << std::endl;
729 }
730 }
731
732
733 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
734 VariableSubMatrixHandler&
InitialAssJac(VariableSubMatrixHandler & WorkMat,const VectorHandler & XCurr)735 PinJoint::InitialAssJac(VariableSubMatrixHandler& WorkMat,
736 const VectorHandler& XCurr)
737 {
738 DEBUGCOUT("Entering PinJoint::InitialAssJac()" << std::endl);
739
740 FullSubMatrixHandler& WM = WorkMat.SetFull();
741
742 /* Dimensiona e resetta la matrice di lavoro */
743 integer iNumRows = 0;
744 integer iNumCols = 0;
745 InitialWorkSpaceDim(&iNumRows, &iNumCols);
746 WM.ResizeReset(iNumRows, iNumCols);
747
748 /* Equazioni: vedi joints.dvi */
749
750 /* Indici */
751 integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
752 integer iFirstVelocityIndex = iFirstPositionIndex+6;
753 integer iFirstReactionIndex = iGetFirstIndex();
754 integer iReactionPrimeIndex = iFirstReactionIndex+3;
755
756 /* Setto gli indici */
757 for (int iCnt = 1; iCnt <= 6; iCnt++) {
758 WM.PutRowIndex(iCnt, iFirstPositionIndex+iCnt);
759 WM.PutColIndex(iCnt, iFirstPositionIndex+iCnt);
760 WM.PutRowIndex(6+iCnt, iFirstVelocityIndex+iCnt);
761 WM.PutColIndex(6+iCnt, iFirstVelocityIndex+iCnt);
762 WM.PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
763 WM.PutColIndex(12+iCnt, iFirstReactionIndex+iCnt);
764 }
765
766 /* Matrici identita' */
767 for (int iCnt = 1; iCnt <= 3; iCnt++) {
768 /* Contributo di forza all'equazione della forza */
769 WM.PutCoef(iCnt, 12+iCnt, 1.);
770
771 /* Contrib. di der. di forza all'eq. della der. della forza */
772 WM.PutCoef(6+iCnt, 15+iCnt, 1.);
773
774 /* Equazione di vincolo */
775 WM.PutCoef(12+iCnt, iCnt, -1.);
776
777 /* Derivata dell'equazione di vincolo */
778 WM.PutCoef(15+iCnt, 6+iCnt, -1.);
779 }
780
781 /* Recupera i dati */
782 const Mat3x3& R(pNode->GetRRef());
783 const Vec3& Omega(pNode->GetWRef());
784 /* F e' stata aggiornata da InitialAssRes */
785 Vec3 FPrime(XCurr, iReactionPrimeIndex+1);
786
787 /* Distanza nel sistema globale */
788 Vec3 dTmp(R*d);
789
790 /* Matrici F/\d/\ */
791 Mat3x3 FWedgedWedge(MatCrossCross, F, dTmp);
792
793 /* Matrici (omega/\d)/\ */
794 Mat3x3 OWedgedWedge(MatCross, Omega.Cross(dTmp));
795
796 /* Equazione di momento */
797 WM.Add(4, 4, FWedgedWedge);
798 WM.Add(4, 13, Mat3x3(MatCross, dTmp));
799
800 /* Derivata dell'equazione di momento */
801 WM.Add(10, 4, (Mat3x3(MatCross, FPrime) + Mat3x3(MatCrossCross, F, Omega))*Mat3x3(MatCross, dTmp));
802 WM.Add(10, 10, FWedgedWedge);
803 WM.Add(10, 13, OWedgedWedge);
804 WM.Add(10, 16, Mat3x3(MatCross, dTmp));
805
806 /* Equazione di vincolo */
807 WM.Add(13, 4, Mat3x3(MatCross, dTmp));
808
809 /* Derivata dell'equazione di vincolo */
810 WM.Add(16, 4, OWedgedWedge);
811 WM.Add(16, 10, Mat3x3(MatCross, dTmp));
812
813 return WorkMat;
814 }
815
816
817 /* Contributo al residuo durante l'assemblaggio iniziale */
818 SubVectorHandler&
InitialAssRes(SubVectorHandler & WorkVec,const VectorHandler & XCurr)819 PinJoint::InitialAssRes(SubVectorHandler& WorkVec,
820 const VectorHandler& XCurr)
821 {
822 DEBUGCOUT("Entering PinJoint::InitialAssRes()" << std::endl);
823
824 /* Dimensiona e resetta la matrice di lavoro */
825 integer iNumRows = 0;
826 integer iNumCols = 0;
827 InitialWorkSpaceDim(&iNumRows, &iNumCols);
828 WorkVec.ResizeReset(iNumRows);
829
830 /* Indici */
831 integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
832 integer iFirstVelocityIndex = iFirstPositionIndex+6;
833 integer iFirstReactionIndex = iGetFirstIndex();
834 integer iReactionPrimeIndex = iFirstReactionIndex+3;
835
836 /* Setta gli indici */
837 for (int iCnt = 1; iCnt <= 6; iCnt++) {
838 WorkVec.PutRowIndex(iCnt, iFirstPositionIndex+iCnt);
839 WorkVec.PutRowIndex(6+iCnt, iFirstVelocityIndex+iCnt);
840 WorkVec.PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
841 }
842
843 /* Recupera i dati */
844 const Vec3& x(pNode->GetXCurr());
845 const Vec3& v(pNode->GetVCurr());
846 const Mat3x3& R(pNode->GetRCurr());
847 const Vec3& Omega(pNode->GetWCurr());
848 F = Vec3(XCurr, iFirstReactionIndex+1);
849 Vec3 FPrime(XCurr, iReactionPrimeIndex+1);
850
851 /* Distanza nel sistema globale */
852 Vec3 dTmp(R*d);
853
854 /* Vettori omega/\d */
855 Vec3 OWedged(Omega.Cross(dTmp));
856
857 /* Equazioni di equilibrio */
858 WorkVec.Sub(1, F);
859 WorkVec.Add(4, F.Cross(dTmp)); /* Sfrutto il fatto che F/\d = -d/\F */
860
861 /* Derivate delle equazioni di equilibrio */
862 WorkVec.Sub(7, FPrime);
863 WorkVec.Add(10, FPrime.Cross(dTmp)-OWedged.Cross(F));
864
865 /* Equazione di vincolo */
866 WorkVec.Add(13, x+dTmp-X0);
867
868 /* Derivata dell'equazione di vincolo */
869 WorkVec.Add(16, v+OWedged);
870
871 return WorkVec;
872 }
873
874 /* PinJoint - end */
875