MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
inplanej.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/inplanej.cc,v 1.34 2017/01/12 14:46:43 masarati Exp $ */
2 /*
3  * MBDyn (C) is a multibody analysis code.
4  * http://www.mbdyn.org
5  *
6  * Copyright (C) 1996-2017
7  *
8  * Pierangelo Masarati <masarati@aero.polimi.it>
9  * Paolo Mantegazza <mantegazza@aero.polimi.it>
10  *
11  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
12  * via La Masa, 34 - 20156 Milano, Italy
13  * http://www.aero.polimi.it
14  *
15  * Changing this copyright notice is forbidden.
16  *
17  * This program is free software; you can redistribute it and/or modify
18  * it under the terms of the GNU General Public License as published by
19  * the Free Software Foundation (version 2 of the License).
20  *
21  *
22  * This program is distributed in the hope that it will be useful,
23  * but WITHOUT ANY WARRANTY; without even the implied warranty of
24  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
25  * GNU General Public License for more details.
26  *
27  * You should have received a copy of the GNU General Public License
28  * along with this program; if not, write to the Free Software
29  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30  */
31 
32 /* Vincoli relativi a movimenti lineari */
33 
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35 
36 #include "inplanej.h"
37 
38 /* InPlaneJoint - begin */
39 
40 /* Costruttore non banale */
41 InPlaneJoint::InPlaneJoint(unsigned int uL, const DofOwner* pDO,
42  const StructNode* pN1, const StructNode* pN2,
43  const Vec3& vTmp, const Vec3& pTmp, flag fOut)
44 : Elem(uL, fOut), Joint(uL, pDO, fOut),
45 pNode1(pN1), pNode2(pN2), v(vTmp), p(pTmp), dF(0.)
46 {
47  NO_OP;
48 };
49 
50 
51 InPlaneJoint::~InPlaneJoint(void)
52 {
53  NO_OP;
54 }
55 
56 /* Contributo al file di restart */
57 std::ostream& InPlaneJoint::Restart(std::ostream& out) const
58 {
59  Joint::Restart(out) << ", in plane, "
60  << pNode1->GetLabel()
61  << ", reference, node, ",
62  p.Write(out, ", ")
63  << ", reference, node, ";
64  v.Write(out, ", ") << ", "
65  << pNode2->GetLabel() << ';' << std::endl;
66  return out;
67 }
68 
69 
70 VariableSubMatrixHandler&
71 InPlaneJoint::AssJac(VariableSubMatrixHandler& WorkMat,
72  doublereal dCoef,
73  const VectorHandler& /* XCurr */ ,
74  const VectorHandler& /* XPrimeCurr */ )
75 {
76  DEBUGCOUT("Entering InPlaneJoint::AssJac()" << std::endl);
77  SparseSubMatrixHandler& WM = WorkMat.SetSparse();
78  WM.ResizeReset(51, 0);
79 
80  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
81  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
82  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
83  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
84  integer iFirstReactionIndex = iGetFirstIndex();
85 
86  Vec3 x2mx1(pNode2->GetXCurr()-pNode1->GetXCurr());
87  Vec3 vTmp(pNode1->GetRRef()*v);
88  Vec3 F(vTmp*(dF*dCoef));
89 
90 
91  Vec3 Tmp(vTmp.Cross(x2mx1));
92  for(int iCnt = 1; iCnt <= 3; iCnt++) {
93  doublereal d = vTmp.dGet(iCnt);
94  WM.PutItem(iCnt, iNode1FirstMomIndex+iCnt,
95  iFirstReactionIndex+1, -d);
96  WM.PutItem(3+iCnt, iNode2FirstMomIndex+iCnt,
97  iFirstReactionIndex+1, d);
98 
99  WM.PutItem(6+iCnt, iFirstReactionIndex+1,
100  iNode1FirstPosIndex+iCnt, -d);
101  WM.PutItem(9+iCnt, iFirstReactionIndex+1,
102  iNode2FirstPosIndex+iCnt, d);
103 
104  d = Tmp.dGet(iCnt);
105  WM.PutItem(12+iCnt, iNode1FirstMomIndex+3+iCnt,
106  iFirstReactionIndex+1, d);
107 
108  WM.PutItem(15+iCnt, iFirstReactionIndex+1,
109  iNode1FirstPosIndex+3+iCnt, d);
110  }
111 
112  WM.PutCross(19, iNode1FirstMomIndex,
113  iNode1FirstPosIndex+3, F);
114  WM.PutCross(25, iNode1FirstMomIndex+3,
115  iNode1FirstPosIndex, -F);
116  WM.PutMat3x3(31, iNode1FirstMomIndex+3,
117  iNode1FirstPosIndex+3, Mat3x3(MatCrossCross, x2mx1, F));
118  WM.PutCross(40, iNode1FirstMomIndex+3,
119  iNode2FirstPosIndex, F);
120  WM.PutCross(46, iNode2FirstMomIndex,
121  iNode2FirstPosIndex+3, -F);
122 
123  return WorkMat;
124 }
125 
126 
127 SubVectorHandler& InPlaneJoint::AssRes(SubVectorHandler& WorkVec,
128  doublereal dCoef,
129  const VectorHandler& XCurr,
130  const VectorHandler& /* XPrimeCurr */ )
131 {
132  DEBUGCOUT("Entering InPlaneJoint::AssRes()" << std::endl);
133  WorkVec.ResizeReset(13);
134 
135  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
136  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
137  integer iFirstReactionIndex = iGetFirstIndex();
138 
139  /* Indici equazioni */
140  for (int iCnt = 1; iCnt <= 6; iCnt++) {
141  WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex+iCnt);
142  WorkVec.PutRowIndex(6+iCnt, iNode2FirstMomIndex+iCnt);
143  }
144 
145  /* Indice equazione vincolo */
146  WorkVec.PutRowIndex(13, iFirstReactionIndex+1);
147 
148  Vec3 x2mx1(pNode2->GetXCurr()-pNode1->GetXCurr());
149  Mat3x3 R(pNode1->GetRCurr());
150  Vec3 vTmp = R*v;
151  // Vec3 pTmp = R*p;
152 
153  /* Aggiorna i dati propri */
154  dF = XCurr(iFirstReactionIndex+1);
155  Vec3 F(vTmp*dF);
156 
157  WorkVec.Add(1, F);
158  WorkVec.Add(4, x2mx1.Cross(F)); /* ( = -p/\F) */
159  WorkVec.Add(7, -F);
160  ASSERT(dCoef != 0.);
161  WorkVec.PutCoef(13, (v.Dot(p)-vTmp.Dot(x2mx1))/dCoef);
162 
163  return WorkVec;
164 }
165 
166 
167 void InPlaneJoint::Output(OutputHandler& OH) const
168 {
169  if(bToBeOutput()) {
170  Vec3 vTmp(pNode1->GetRCurr()*v);
171  Joint::Output(OH.Joints(), "InPlane", GetLabel(),
172  Vec3(dF, 0., 0.), Zero3, vTmp*dF, Zero3) << std::endl;
173  }
174 }
175 
176 
177 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
178 VariableSubMatrixHandler&
179 InPlaneJoint::InitialAssJac(VariableSubMatrixHandler& WorkMat,
180  const VectorHandler& XCurr)
181 {
182  DEBUGCOUT("Entering InPlaneJoint::InitialAssJac()" << std::endl);
183  FullSubMatrixHandler& WM = WorkMat.SetFull();
184  WM.ResizeReset(26, 26);
185 
186  /* Indici gdl */
187  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
188  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
189  integer iFirstReactionIndex = iGetFirstIndex();
190  integer iReactionPrimeIndex = iFirstReactionIndex+1;
191 
192  /* Indici equazioni nodi */
193  for(int iCnt = 1; iCnt <= 12; iCnt++) {
194  WM.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
195  WM.PutColIndex(iCnt, iNode1FirstPosIndex+iCnt);
196  WM.PutRowIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
197  WM.PutColIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
198  }
199 
200  /* Indici vincoli */
201  for(int iCnt = 1; iCnt <= 2; iCnt++) {
202  WM.PutRowIndex(24+iCnt, iFirstReactionIndex+iCnt);
203  WM.PutColIndex(24+iCnt, iFirstReactionIndex+iCnt);
204  }
205 
206  /* Dati */
207  Vec3 x2mx1(pNode2->GetXCurr()-pNode1->GetXCurr());
208  Vec3 xp2mxp1(pNode2->GetVCurr()-pNode1->GetVCurr());
209  Vec3 Omega(pNode1->GetWRef());
210 
211  /* Aggiorna i dati propri */
212  doublereal dFPrime = XCurr(iReactionPrimeIndex+1);
213  Vec3 vTmp(pNode1->GetRRef()*v);
214  Vec3 F(vTmp*dF);
215  Vec3 FPrime(vTmp*dFPrime);
216 
217  Vec3 Tmp1(vTmp.Cross(x2mx1));
218  Vec3 Tmp2(Omega.Cross(vTmp));
219  Vec3 Tmp3((Omega.Cross(x2mx1)-xp2mxp1).Cross(vTmp));
220  Vec3 Tmp4(-(xp2mxp1.Cross(vTmp)+x2mx1.Cross(Tmp2)));
221  for(int iCnt = 1; iCnt <= 3; iCnt++) {
222  doublereal d = vTmp.dGet(iCnt);
223  WM.PutCoef(iCnt, 25, -d);
224  WM.PutCoef(12+iCnt, 25, d);
225 
226  WM.PutCoef(25, iCnt, -d);
227  WM.PutCoef(25, 12+iCnt, d);
228 
229  WM.PutCoef(6+iCnt, 26, -d);
230  WM.PutCoef(18+iCnt, 26, d);
231 
232  WM.PutCoef(26, 6+iCnt, -d);
233  WM.PutCoef(26, 18+iCnt, d);
234 
235  d = Tmp1.dGet(iCnt);
236  WM.PutCoef(3+iCnt, 25, d);
237  WM.PutCoef(25, 3+iCnt, d);
238 
239  WM.PutCoef(26, 9+iCnt, d);
240  WM.PutCoef(9+iCnt, 26, d);
241 
242  d = Tmp2.dGet(iCnt);
243  WM.PutCoef(26, iCnt, -d);
244  WM.PutCoef(26, 12+iCnt, d);
245 
246  WM.PutCoef(6+iCnt, 25, -d);
247  WM.PutCoef(18+iCnt, 25, d);
248 
249  d = Tmp3.dGet(iCnt);
250  WM.PutCoef(26, 3+iCnt, d);
251 
252  d = Tmp4.dGet(iCnt);
253  WM.PutCoef(9+iCnt, 25, d);
254  }
255 
256  Mat3x3 MTmp(MatCross, F);
257  WM.Add(1, 4, MTmp);
258  WM.Add(4, 13, MTmp);
259 
260  WM.Add(7, 10, MTmp);
261  WM.Add(10, 19, MTmp);
262 
263  WM.Sub(4, 1, MTmp);
264  WM.Sub(13, 4, MTmp);
265 
266  WM.Sub(19, 10, MTmp);
267  WM.Sub(10, 7, MTmp);
268 
269  MTmp = Mat3x3(MatCrossCross, x2mx1, F);
270  WM.Add(4, 4, MTmp);
271 
272  WM.Add(10, 10, MTmp);
273 
274  MTmp = Mat3x3(MatCrossCross, Omega, F) + Mat3x3(MatCross, FPrime);
275  WM.Add(7, 4, MTmp);
276  WM.Sub(19, 4, MTmp);
277 
278  MTmp = Mat3x3(MatCross, Omega.Cross(F) + FPrime);
279  WM.Sub(10, 1, MTmp);
280  WM.Add(10, 13, MTmp);
281 
282  MTmp = Mat3x3((Mat3x3(MatCross, xp2mxp1) + Mat3x3(MatCrossCross, x2mx1, Omega))*Mat3x3(MatCross, F)
283  + Mat3x3(MatCrossCross, x2mx1, FPrime));
284  WM.Add(10, 4, MTmp);
285 
286  return WorkMat;
287 }
288 
289 
290 /* Contributo al residuo durante l'assemblaggio iniziale */
291 SubVectorHandler& InPlaneJoint::InitialAssRes(SubVectorHandler& WorkVec,
292  const VectorHandler& XCurr)
293 {
294  DEBUGCOUT("Entering InPlaneJoint::InitialAssRes()" << std::endl);
295  WorkVec.ResizeReset(26);
296 
297  /* Indici gdl */
298  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
299  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
300  integer iFirstReactionIndex = iGetFirstIndex();
301  integer iReactionPrimeIndex = iFirstReactionIndex+1;
302 
303  /* Indici equazioni nodi */
304  for(int iCnt = 1; iCnt <= 12; iCnt++) {
305  WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
306  WorkVec.PutRowIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
307  }
308 
309  /* Indici equazioni vincoli */
310  WorkVec.PutRowIndex(25, iFirstReactionIndex+1);
311  WorkVec.PutRowIndex(26, iReactionPrimeIndex+1);
312 
313  /* Dati */
314  Vec3 x2mx1(pNode2->GetXCurr()-pNode1->GetXCurr());
315  Vec3 xp2mxp1(pNode2->GetVCurr()-pNode1->GetVCurr());
316  Mat3x3 R(pNode1->GetRCurr());
317  Vec3 Omega(pNode1->GetWCurr());
318  Vec3 vTmp = R*v;
319  // Vec3 pTmp = R*p;
320 
321  /* Aggiorna i dati propri */
322  dF = XCurr(iFirstReactionIndex+1);
323  doublereal dFPrime = XCurr(iReactionPrimeIndex+1);
324  Vec3 F(vTmp*dF);
325  Vec3 FPrime(vTmp*dFPrime);
326  Vec3 Tmp(Omega.Cross(F)+FPrime);
327 
328  WorkVec.Add(1, F);
329  WorkVec.Add(4, x2mx1.Cross(F)); /* ( = -p/\F) */
330  WorkVec.Add(7, Tmp);
331  WorkVec.Add(10, xp2mxp1.Cross(F)+x2mx1.Cross(Tmp));
332  WorkVec.Add(13, -F);
333  WorkVec.Add(19, -Tmp);
334 
335  WorkVec.PutCoef(25, v.Dot(p)-vTmp.Dot(x2mx1));
336  WorkVec.PutCoef(26, x2mx1.Dot(vTmp.Cross(Omega))-vTmp.Dot(xp2mxp1));
337 
338  return WorkVec;
339 }
340 
341 
342 /* Setta il valore iniziale delle proprie variabili */
343 void InPlaneJoint::SetInitialValue(VectorHandler& /* X */ )
344 {
345  NO_OP;
346 }
347 
348 /* InPlaneJoint - end */
349 
350 
351 /* InPlaneWithOffsetJoint - begin */
352 
353 /* Costruttore non banale */
354 InPlaneWithOffsetJoint::InPlaneWithOffsetJoint(unsigned int uL,
355  const DofOwner* pDO,
356  const StructNode* pN1,
357  const StructNode* pN2,
358  const Vec3& vT,
359  const Vec3& pT,
360  const Vec3& qT,
361  flag fOut)
362 : Elem(uL, fOut), Joint(uL, pDO, fOut),
363 pNode1(pN1), pNode2(pN2), v(vT), p(pT), q(qT), dF(0.)
364 {
365  NO_OP;
366 };
367 
368 
369 InPlaneWithOffsetJoint::~InPlaneWithOffsetJoint(void)
370 {
371  NO_OP;
372 }
373 
374 /* Contributo al file di restart */
375 std::ostream& InPlaneWithOffsetJoint::Restart(std::ostream& out) const
376 {
377  Joint::Restart(out) << ", in plane, "
378  << pNode1->GetLabel()
379  << ", reference, node, ",
380  p.Write(out, ", ")
381  << ", reference, node, ",
382  v.Write(out, ", ") << ", "
383  << pNode2->GetLabel() << ", offset, reference, node, ";
384  return q.Write(out, ", ") << ';' << std::endl;
385 }
386 
387 
388 VariableSubMatrixHandler&
389 InPlaneWithOffsetJoint::AssJac(VariableSubMatrixHandler& WorkMat,
390  doublereal dCoef,
391  const VectorHandler& /* XCurr */ ,
392  const VectorHandler& /* XPrimeCurr */ )
393 {
394  DEBUGCOUT("Entering InPlaneWithOffsetJoint::AssJac()" << std::endl);
395  SparseSubMatrixHandler& WM = WorkMat.SetSparse();
396  WM.ResizeReset(84, 0);
397 
398  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
399  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
400  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
401  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
402  integer iFirstReactionIndex = iGetFirstIndex();
403 
404  Vec3 vTmp(pNode1->GetRRef()*v);
405  Vec3 qTmp(pNode2->GetRRef()*q);
406  Vec3 x2pqmx1(pNode2->GetXCurr()+qTmp-pNode1->GetXCurr());
407  Vec3 F(vTmp*(dF*dCoef));
408 
409 
410  Vec3 Tmp1(vTmp.Cross(x2pqmx1));
411  Vec3 Tmp2(qTmp.Cross(vTmp));
412  for(int iCnt = 1; iCnt <= 3; iCnt++) {
413  doublereal d = vTmp.dGet(iCnt);
414  WM.PutItem(iCnt, iNode1FirstMomIndex+iCnt,
415  iFirstReactionIndex+1, -d);
416  WM.PutItem(3+iCnt, iNode2FirstMomIndex+iCnt,
417  iFirstReactionIndex+1, d);
418 
419  WM.PutItem(6+iCnt, iFirstReactionIndex+1,
420  iNode1FirstPosIndex+iCnt, -d);
421  WM.PutItem(9+iCnt, iFirstReactionIndex+1,
422  iNode2FirstPosIndex+iCnt, d);
423 
424  d = Tmp1.dGet(iCnt);
425  WM.PutItem(12+iCnt, iNode1FirstMomIndex+3+iCnt,
426  iFirstReactionIndex+1, d);
427 
428  WM.PutItem(15+iCnt, iFirstReactionIndex+1,
429  iNode1FirstPosIndex+3+iCnt, d);
430 
431  d = Tmp2.dGet(iCnt);
432  WM.PutItem(18+iCnt, iNode2FirstMomIndex+3+iCnt,
433  iFirstReactionIndex+1, d);
434 
435  WM.PutItem(21+iCnt, iFirstReactionIndex+1,
436  iNode2FirstPosIndex+3+iCnt, d);
437  }
438 
439  WM.PutCross(25, iNode1FirstMomIndex,
440  iNode1FirstPosIndex+3, F);
441  WM.PutCross(31, iNode1FirstMomIndex+3,
442  iNode1FirstPosIndex, -F);
443  WM.PutMat3x3(37, iNode1FirstMomIndex+3,
444  iNode1FirstPosIndex+3, Mat3x3(MatCrossCross, x2pqmx1, F));
445  WM.PutCross(46, iNode1FirstMomIndex+3,
446  iNode2FirstPosIndex, F);
447  WM.PutCross(52, iNode2FirstMomIndex,
448  iNode2FirstPosIndex+3, -F);
449 
450  Mat3x3 MTmp(MatCrossCross, F, qTmp);
451  WM.PutMat3x3(58, iNode1FirstMomIndex+3,
452  iNode2FirstPosIndex+3, -MTmp);
453  WM.PutMat3x3(67, iNode2FirstMomIndex+3,
454  iNode2FirstPosIndex+3, -MTmp);
455 
456  WM.PutMat3x3(76, iNode2FirstMomIndex+3,
457  iNode1FirstPosIndex+3, Mat3x3(MatCrossCross, -qTmp, F));
458 
459  return WorkMat;
460 }
461 
462 
463 SubVectorHandler&
464 InPlaneWithOffsetJoint::AssRes(SubVectorHandler& WorkVec,
465  doublereal dCoef,
466  const VectorHandler& XCurr,
467  const VectorHandler& /* XPrimeCurr */ )
468 {
469  DEBUGCOUT("Entering InPlaneWithOffsetJoint::AssRes()" << std::endl);
470  WorkVec.ResizeReset(13);
471 
472  // integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
473  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
474  // integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
475  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
476  integer iFirstReactionIndex = iGetFirstIndex();
477 
478  /* Indici equazioni */
479  for(int iCnt = 1; iCnt <= 6; iCnt++) {
480  WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex+iCnt);
481  WorkVec.PutRowIndex(6+iCnt, iNode2FirstMomIndex+iCnt);
482  }
483 
484  /* Indice equazione vincolo */
485  WorkVec.PutRowIndex(13, iFirstReactionIndex+1);
486 
487  Vec3 vTmp(pNode1->GetRCurr()*v);
488  Vec3 qTmp(pNode2->GetRCurr()*q);
489  Vec3 x2pqmx1(pNode2->GetXCurr()+qTmp-pNode1->GetXCurr());
490 
491  /* Aggiorna i dati propri */
492  dF = XCurr(iFirstReactionIndex+1);
493  Vec3 F(vTmp*dF);
494 
495  WorkVec.Add(1, F);
496  WorkVec.Add(4, x2pqmx1.Cross(F));
497  WorkVec.Add(7, -F);
498  WorkVec.Add(10, F.Cross(qTmp));
499  if(dCoef != 0.) {
500  WorkVec.PutCoef(13, (v.Dot(p)-vTmp.Dot(x2pqmx1))/dCoef);
501  }
502 
503  return WorkVec;
504 }
505 
506 
507 void InPlaneWithOffsetJoint::Output(OutputHandler& OH) const
508 {
509  if(bToBeOutput()) {
510  Vec3 vTmp(pNode1->GetRCurr()*v);
511  Joint::Output(OH.Joints(), "InPlaneWithOffs", GetLabel(),
512  Vec3(dF, 0., 0.), Zero3, vTmp*dF, Zero3) << std::endl;
513  }
514 }
515 
516 
517 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
518 VariableSubMatrixHandler&
519 InPlaneWithOffsetJoint::InitialAssJac(VariableSubMatrixHandler& WorkMat,
520  const VectorHandler& XCurr)
521 {
522  DEBUGCOUT("Entering InPlaneWithOffsetJoint::InitialAssJac()" << std::endl);
523  FullSubMatrixHandler& WM = WorkMat.SetFull();
524  WM.ResizeReset(26, 26);
525 
526  /* Indici gdl */
527  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
528  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
529  integer iFirstReactionIndex = iGetFirstIndex();
530  integer iReactionPrimeIndex = iFirstReactionIndex+1;
531 
532  /* Indici equazioni nodi */
533  for(int iCnt = 1; iCnt <= 12; iCnt++) {
534  WM.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
535  WM.PutColIndex(iCnt, iNode1FirstPosIndex+iCnt);
536  WM.PutRowIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
537  WM.PutColIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
538  }
539 
540  /* Indici vincoli */
541  for(int iCnt = 1; iCnt <= 2; iCnt++) {
542  WM.PutRowIndex(24+iCnt, iFirstReactionIndex+iCnt);
543  WM.PutColIndex(24+iCnt, iFirstReactionIndex+iCnt);
544  }
545 
546  /* Dati */
547  Vec3 Omega1(pNode1->GetWRef());
548  Vec3 Omega2(pNode2->GetWRef());
549  Vec3 vTmp(pNode1->GetRRef()*v);
550  Vec3 qTmp(pNode2->GetRRef()*q);
551  Vec3 x2pqmx1(pNode2->GetXCurr()+qTmp-pNode1->GetXCurr());
552  Vec3 xp2pqpmxp1(pNode2->GetVCurr()+Omega2.Cross(qTmp)-pNode1->GetVCurr());
553 
554  /* Aggiorna i dati propri */
555  doublereal dFPrime = XCurr(iReactionPrimeIndex+1);
556  Vec3 F(vTmp*dF);
557  Vec3 FPrime(vTmp*dFPrime);
558 
559  Vec3 Tmp1(vTmp.Cross(x2pqmx1));
560  Vec3 Tmp2(Omega1.Cross(vTmp));
561  Vec3 Tmp3((Omega1.Cross(x2pqmx1) - xp2pqpmxp1).Cross(vTmp));
562  Vec3 Tmp4(-(xp2pqpmxp1.Cross(vTmp) + x2pqmx1.Cross(Tmp2)));
563 
564  Vec3 Tmp5(qTmp.Cross(vTmp));
565  Vec3 Tmp6(qTmp.Cross(vTmp.Cross(Omega2-Omega1)));
566  Vec3 Tmp7(qTmp.Cross(Omega1.Cross(vTmp)) - vTmp.Cross(Omega2.Cross(qTmp)));
567  for(int iCnt = 1; iCnt <= 3; iCnt++) {
568  doublereal d = vTmp.dGet(iCnt);
569  WM.PutCoef(iCnt, 25, -d);
570  WM.PutCoef(12+iCnt, 25, d);
571 
572  WM.PutCoef(25, iCnt, -d);
573  WM.PutCoef(25, 12+iCnt, d);
574 
575  WM.PutCoef(6+iCnt, 26, -d);
576  WM.PutCoef(18+iCnt, 26, d);
577 
578  WM.PutCoef(26, 6+iCnt, -d);
579  WM.PutCoef(26, 18+iCnt, d);
580 
581  d = Tmp1.dGet(iCnt);
582  WM.PutCoef(3+iCnt, 25, d);
583  WM.PutCoef(25, 3+iCnt, d);
584 
585  WM.PutCoef(26, 9+iCnt, d);
586  WM.PutCoef(9+iCnt, 26, d);
587 
588  d = Tmp2.dGet(iCnt);
589  WM.PutCoef(26, iCnt, -d);
590  WM.PutCoef(26, 12+iCnt, d);
591 
592  WM.PutCoef(6+iCnt, 25, -d);
593  WM.PutCoef(18+iCnt, 25, d);
594 
595  d = Tmp3.dGet(iCnt);
596  WM.PutCoef(26, 3+iCnt, d);
597 
598  d = Tmp4.dGet(iCnt);
599  WM.PutCoef(9+iCnt, 25, d);
600 
601  d = Tmp5.dGet(iCnt);
602  WM.PutCoef(15+iCnt, 25, d);
603  WM.PutCoef(21+iCnt, 26, d);
604 
605  WM.PutCoef(25, 15+iCnt, d);
606  WM.PutCoef(26, 21+iCnt, d);
607 
608  d = Tmp6.dGet(iCnt);
609  WM.PutCoef(26, 15+iCnt, d);
610 
611  d = Tmp7.dGet(iCnt);
612  WM.PutCoef(21+iCnt, 25, d);
613  }
614 
615  Mat3x3 MTmp(MatCross, F);
616  WM.Add(1, 4, MTmp);
617  WM.Add(4, 13, MTmp);
618 
619  WM.Add(7, 10, MTmp);
620  WM.Add(10, 19, MTmp);
621 
622  WM.Sub(4, 1, MTmp);
623  WM.Sub(13, 4, MTmp);
624 
625  WM.Sub(19, 10, MTmp);
626  WM.Sub(10, 7, MTmp);
627 
628  MTmp = Mat3x3(MatCrossCross, x2pqmx1, F);
629  WM.Add(4, 4, MTmp);
630 
631  WM.Add(10, 10, MTmp);
632 
633  MTmp = Mat3x3(MatCrossCross, Omega1, F) + Mat3x3(MatCross, FPrime);
634  WM.Add(7, 4, MTmp);
635  WM.Sub(19, 4, MTmp);
636 
637  MTmp = Mat3x3(MatCross, Omega1.Cross(F) + FPrime);
638  WM.Sub(10, 1, MTmp);
639  WM.Add(10, 13, MTmp);
640 
641  MTmp = (Mat3x3(MatCross, xp2pqpmxp1) + Mat3x3(MatCrossCross, x2pqmx1, Omega1))*Mat3x3(MatCross, F)
642  + Mat3x3(MatCrossCross, x2pqmx1, FPrime);
643  WM.Add(10, 4, MTmp);
644 
645  MTmp = Mat3x3(MatCrossCross, F, qTmp);
646  WM.Add(16, 16, MTmp);
647  WM.Add(22, 22, MTmp);
648 
649  WM.Sub(4, 16, MTmp);
650  WM.Sub(10, 22, MTmp);
651 
652  MTmp = MTmp.Transpose();
653  WM.Add(16, 4, MTmp);
654  WM.Add(22, 10, MTmp);
655 
656  MTmp = (Mat3x3(MatCrossCross, F, Omega2) + Mat3x3(MatCross, Omega1.Cross(F) + FPrime))*Mat3x3(MatCross, qTmp);
657  WM.Add(22, 16, MTmp);
658  WM.Sub(10, 16, MTmp);
659 
660  MTmp = Mat3x3(MatCrossCross, qTmp.Cross(Omega2), F)
661  - qTmp.Cross(Mat3x3(MatCrossCross, Omega1, F) + Mat3x3(MatCross, FPrime));
662  WM.Add(22, 4, MTmp);
663 
664  return WorkMat;
665 }
666 
667 
668 /* Contributo al residuo durante l'assemblaggio iniziale */
669 SubVectorHandler&
670 InPlaneWithOffsetJoint::InitialAssRes(SubVectorHandler& WorkVec,
671  const VectorHandler& XCurr)
672 {
673  DEBUGCOUT("Entering InPlaneWithOffsetJoint::InitialAssRes()" << std::endl);
674  WorkVec.ResizeReset(26);
675 
676  /* Indici gdl */
677  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
678  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
679  integer iFirstReactionIndex = iGetFirstIndex();
680  integer iReactionPrimeIndex = iFirstReactionIndex+1;
681 
682  /* Indici equazioni nodi */
683  for(int iCnt = 1; iCnt <= 12; iCnt++) {
684  WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
685  WorkVec.PutRowIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
686  }
687 
688  /* Indici equazioni vincoli */
689  WorkVec.PutRowIndex(25, iFirstReactionIndex+1);
690  WorkVec.PutRowIndex(26, iReactionPrimeIndex+1);
691 
692  /* Dati */
693  Vec3 Omega1(pNode1->GetWCurr());
694  Vec3 Omega2(pNode2->GetWCurr());
695  Vec3 vTmp(pNode1->GetRCurr()*v);
696  Vec3 qTmp(pNode2->GetRCurr()*q);
697 
698  Vec3 x2pqmx1(pNode2->GetXCurr()+qTmp-pNode1->GetXCurr());
699  Vec3 xp2pqpmxp1(pNode2->GetVCurr()+Omega2.Cross(qTmp)-pNode1->GetVCurr());
700 
701  /* Aggiorna i dati propri */
702  dF = XCurr(iFirstReactionIndex+1);
703  doublereal dFPrime = XCurr(iReactionPrimeIndex+1);
704  Vec3 F(vTmp*dF);
705  Vec3 FPrime(vTmp*dFPrime);
706  Vec3 Tmp(Omega1.Cross(F)+FPrime);
707 
708  WorkVec.Add(1, F);
709  WorkVec.Add(4, x2pqmx1.Cross(F));
710  WorkVec.Add(7, Tmp);
711  WorkVec.Add(10, xp2pqpmxp1.Cross(F)+x2pqmx1.Cross(Tmp));
712  WorkVec.Add(13, -F);
713  WorkVec.Add(16, F.Cross(qTmp));
714  WorkVec.Add(19, -Tmp);
715  WorkVec.Add(22, F.Cross(Omega2.Cross(qTmp))-qTmp.Cross(Tmp));
716 
717  WorkVec.PutCoef(25, v.Dot(p)-vTmp.Dot(x2pqmx1));
718  WorkVec.PutCoef(26, x2pqmx1.Dot(vTmp.Cross(Omega1))
719  -vTmp.Dot(xp2pqpmxp1));
720 
721  return WorkVec;
722 }
723 
724 
725 /* Setta il valore iniziale delle proprie variabili */
726 void InPlaneWithOffsetJoint::SetInitialValue(VectorHandler& /* X */ )
727 {
728  NO_OP;
729 }
730 
731 /* InPlaneWithOffsetJoint - end */
FullSubMatrixHandler::PutColIndex
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
InPlaneJoint::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: inplanej.cc:57
SparseSubMatrixHandler::PutMat3x3
void PutMat3x3(integer iSubIt, integer iFirstRow, integer iFirstCol, const Mat3x3 &m)
Definition: submat.cc:1331
Zero3
const Vec3 Zero3(0., 0., 0.)
Vec3::Cross
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
Vec3::Write
std::ostream & Write(std::ostream &out, const char *sFill=" ") const
Definition: matvec3.cc:738
flag
long int flag
Definition: mbdyn.h:43
StructNode::GetRRef
virtual const Mat3x3 & GetRRef(void) const
Definition: strnode.h:1006
ToBeOutput::bToBeOutput
virtual bool bToBeOutput(void) const
Definition: output.cc:890
Vec3
Definition: matvec3.h:98
VectorHandler::ResizeReset
virtual void ResizeReset(integer)
Definition: vh.cc:55
InPlaneJoint::pNode2
const StructNode * pNode2
Definition: inplanej.h:55
MatCross
const MatCross_Manip MatCross
Definition: matvec3.cc:639
InPlaneWithOffsetJoint::InitialAssRes
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: inplanej.cc:670
VariableSubMatrixHandler::SetFull
FullSubMatrixHandler & SetFull(void)
Definition: submat.h:1168
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
InPlaneJoint::p
const Vec3 p
Definition: inplanej.h:58
Vec3::Dot
doublereal Dot(const Vec3 &v) const
Definition: matvec3.h:243
InPlaneWithOffsetJoint::pNode2
const StructNode * pNode2
Definition: inplanej.h:159
FullSubMatrixHandler::Add
void Add(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:209
InPlaneWithOffsetJoint::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: inplanej.cc:375
InPlaneJoint::AssJac
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: inplanej.cc:71
FullSubMatrixHandler::PutCoef
void PutCoef(integer iRow, integer iCol, const doublereal &dCoef)
Definition: submat.h:672
SparseSubMatrixHandler::ResizeReset
void ResizeReset(integer iNewRow, integer iNewCol)
Definition: submat.cc:1084
StructNode::GetWRef
virtual const Vec3 & GetWRef(void) const
Definition: strnode.h:1024
NO_OP
#define NO_OP
Definition: myassert.h:74
SparseSubMatrixHandler::PutCross
void PutCross(integer iSubIt, integer iFirstRow, integer iFirstCol, const Vec3 &v)
Definition: submat.cc:1236
InPlaneWithOffsetJoint::InitialAssJac
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: inplanej.cc:519
SubVectorHandler::PutRowIndex
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
InPlaneJoint::Output
virtual void Output(OutputHandler &OH) const
Definition: inplanej.cc:167
SparseSubMatrixHandler::PutItem
void PutItem(integer iSubIt, integer iRow, integer iCol, const doublereal &dCoef)
Definition: submat.h:997
InPlaneWithOffsetJoint::SetInitialValue
virtual void SetInitialValue(VectorHandler &X)
Definition: inplanej.cc:726
InPlaneWithOffsetJoint::AssRes
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: inplanej.cc:464
Vec3::dGet
const doublereal & dGet(unsigned short int iRow) const
Definition: matvec3.h:285
DEBUGCOUT
#define DEBUGCOUT(msg)
Definition: myassert.h:232
StructDispNode::iGetFirstMomentumIndex
virtual integer iGetFirstMomentumIndex(void) const =0
StructDispNode::iGetFirstPositionIndex
virtual integer iGetFirstPositionIndex(void) const
Definition: strnode.h:452
StructNode::GetWCurr
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
InPlaneJoint::SetInitialValue
virtual void SetInitialValue(VectorHandler &X)
Definition: inplanej.cc:343
Joint::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: joint.h:195
OutputHandler::Joints
std::ostream & Joints(void) const
Definition: output.h:443
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
InPlaneJoint::dF
doublereal dF
Definition: inplanej.h:60
VectorHandler::PutCoef
virtual void PutCoef(integer iRow, const doublereal &dCoef)=0
grad::Cross
VectorExpression< VectorCrossExpr< VectorLhsExpr, VectorRhsExpr >, 3 > Cross(const VectorExpression< VectorLhsExpr, 3 > &u, const VectorExpression< VectorRhsExpr, 3 > &v)
Definition: matvec.h:3248
InPlaneJoint::AssRes
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: inplanej.cc:127
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
VectorHandler::Add
virtual void Add(integer iRow, const Vec3 &v)
Definition: vh.cc:63
InPlaneJoint::~InPlaneJoint
~InPlaneJoint(void)
Definition: inplanej.cc:51
FullSubMatrixHandler::ResizeReset
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
Mat3x3::Transpose
Mat3x3 Transpose(void) const
Definition: matvec3.h:816
MatCrossCross
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
Elem
Definition: elem.h:75
InPlaneWithOffsetJoint::InPlaneWithOffsetJoint
InPlaneWithOffsetJoint(unsigned int uL, const DofOwner *pDO, const StructNode *pN1, const StructNode *pN2, const Vec3 &vT, const Vec3 &pT, const Vec3 &qT, flag fOut)
Definition: inplanej.cc:354
FullSubMatrixHandler::PutRowIndex
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
InPlaneJoint::InPlaneJoint
InPlaneJoint(unsigned int uL, const DofOwner *pDO, const StructNode *pN1, const StructNode *pN2, const Vec3 &vTmp, const Vec3 &pTmp, flag fOut)
Definition: inplanej.cc:41
InPlaneWithOffsetJoint::Output
virtual void Output(OutputHandler &OH) const
Definition: inplanej.cc:507
InPlaneJoint::v
const Vec3 v
Definition: inplanej.h:57
StructDispNode::GetVCurr
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
FullSubMatrixHandler::Sub
void Sub(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:215
Joint
Definition: joint.h:50
InPlaneJoint::pNode1
const StructNode * pNode1
Definition: inplanej.h:54
InPlaneWithOffsetJoint::AssJac
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: inplanej.cc:389
InPlaneJoint::InitialAssRes
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: inplanej.cc:291
VariableSubMatrixHandler::SetSparse
SparseSubMatrixHandler & SetSparse(void)
Definition: submat.h:1178
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
doublereal
double doublereal
Definition: colamd.c:52
Joint::Output
std::ostream & Output(std::ostream &out, const char *sJointName, unsigned int uLabel, const Vec3 &FLocal, const Vec3 &MLocal, const Vec3 &FGlobal, const Vec3 &MGlobal) const
Definition: joint.cc:138
InPlaneWithOffsetJoint::pNode1
const StructNode * pNode1
Definition: inplanej.h:158
integer
long int integer
Definition: colamd.c:51
WithLabel::GetLabel
unsigned int GetLabel(void) const
Definition: withlab.cc:62
R
Mat3x3 R
Definition: mbdyn_util_oct.cc:74
InPlaneJoint::InitialAssJac
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: inplanej.cc:179