MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
rodbezj.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/rodbezj.cc,v 1.5 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  * Author: Andrea Zanoni <andrea.zanoni@polimi.it>
33  */
34 
35 
36 /* Bezier Rod */
37 
38 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
39 
40 #include <limits>
41 #include <cmath>
42 #include <limits>
43 
44 #include "dataman.h"
45 #include "gauss.h"
46 
47 #include "rodbezj.h"
48 
49 /* Rod Bezier - begin */
50 
51 /* Costructor */
52 RodBezier::RodBezier(unsigned int uL, const DofOwner* pDO,
53  const ConstitutiveLaw1D* pCL,
54  const StructNode* pN1, const StructNode* pN2,
55  const Vec3& fOTmp, const Vec3& fATmp,
56  const Vec3& fBTmp, const Vec3& fITmp,
57  doublereal dLength, bool bFromNodes,
58  const unsigned int iIntOrder, const unsigned int iIntSegments,
59  flag fOut)
60 : Elem(uL, fOut),
61 Joint(uL, pDO, fOut),
62 ConstitutiveLaw1DOwner(pCL),
63 pNode1(pN1),
64 pNode2(pN2),
65 fO(fOTmp),
66 fA(fATmp),
67 fB(fBTmp),
68 fI(fITmp),
69 dL0(dLength),
70 l1(Zero3),
71 l2(Zero3),
72 dElle(0.),
73 dEpsilon(0.),
74 dEpsilonPrime(0.),
75 iIntOrd(iIntOrder),
76 iIntSeg(iIntSegments)
77 {
78  /* Check initial data coherence */
79  ASSERT(pN1 != NULL);
80  ASSERT(pN1->GetNodeType() == Node::STRUCTURAL);
81  ASSERT(pN2 != NULL);
82  ASSERT(pN2->GetNodeType() == Node::STRUCTURAL);
83 
84  /* Integration sites and weights (for curve length)
85  * Gauss-Legendre quadrature used. */
86 
87  gdi = new GaussDataIterator(iIntOrder);
88 
89  const Mat3x3& R1(pN1->GetRRef());
90  const Mat3x3& R2(pN2->GetRRef());
91 
92  l1 = R1*(fATmp - fOTmp);
93  l2 = R2*(fBTmp - fITmp);
94 
95  l1 = l1/l1.Norm();
96  l2 = l2/l2.Norm();
97 
98  if (bFromNodes) {
99 
100  const Vec3 b1 = pN1->GetXCurr() + R1*fO;
101  const Vec3 b2 = pN1->GetXCurr() + R1*fA;
102  const Vec3 b3 = pN2->GetXCurr() + R2*fB;
103  const Vec3 b4 = pN2->GetXCurr() + R2*fI;
104 
105  Vec3 p;
106  doublereal q, up;
107  doublereal s = 1.0/(2.0*iIntSegments);
108 
109  for (unsigned int jj = 0; jj < iIntSegments; jj++) {
110  q = s*(2*jj + 1);
111 
112  PntWght gp = gdi->GetFirst();
113  do {
114  up = s*gp.dGetPnt() + q;
115  p = b1*Ap1(up) + b2*Ap2(up) + b3*Ap3(up) + b4*Ap4(up);
116  dLength = dLength + s*gp.dGetWght()*p.Norm();
117 
118  } while (gdi->fGetNext(gp));
119  }
120 
121  dL0 = dLength;
122 
123  }
124 
125  ASSERT(dLength > std::numeric_limits<doublereal>::epsilon());
126 
127 }
128 
129 /* Destructor */
130 RodBezier::~RodBezier(void)
131 {
132  NO_OP;
133 }
134 
135 /* Restart file contribute */
136 std::ostream&
137 RodBezier::Restart(std::ostream& out) const
138 {
139  Joint::Restart(out);
140  out << ", rod bezier, "
141  << pNode1->GetLabel() << ", "
142  << "position, reference, node, ";
143  fO.Write(out, ", ") << ", "
144  << "position, reference, node, "; fA.Write(out, ", ") << "," << std::endl
145  << pNode2->GetLabel() << ", "
146  << "position, reference, node, "; fO.Write(out, ", ") << ", "
147  << "position, reference, node, "; fA.Write(out, ", ") << "," << std::endl
148  << dL0 << "," << std::endl
149  << "integration order, " << iIntOrd << ", "
150  << "integration segments, " << iIntSeg << ", ";
151  return pGetConstLaw()->Restart(out) << ';' << std::endl;
152 }
153 
154 void
155 RodBezier::AfterConvergence(const VectorHandler& X,
156  const VectorHandler& XP)
157 {
158  ConstitutiveLaw1DOwner::AfterConvergence(dEpsilon, dEpsilonPrime);
159 }
160 
161 /* Jacobian matrix contribute */
162 VariableSubMatrixHandler&
163 RodBezier::AssJac(VariableSubMatrixHandler& WorkMat,
164  doublereal dCoef,
165  const VectorHandler& /* XCurr */ ,
166  const VectorHandler& /* XPrimeCurr */ )
167 {
168  DEBUGCOUT("Entering RodBezier::AssJac()" << std::endl);
169 
170  FullSubMatrixHandler& WM = WorkMat.SetFull();
171 
172  /* Resizes and resets the work matrix */
173  integer iNumRows = 0;
174  integer iNumCols = 0;
175  WorkSpaceDim(&iNumRows, &iNumCols);
176  WM.ResizeReset(iNumRows, iNumCols);
177 
178  /* Gets the nodes indexes */
179  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
180  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
181  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
182  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
183 
184  /* Sets the matrix indexes */
185  for (int iCnt = 1; iCnt <= 6; iCnt++) {
186  WM.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
187  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
188  WM.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
189  WM.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
190  }
191 
192  const Mat3x3& R1(pNode1->GetRRef());
193  const Mat3x3& R2(pNode2->GetRRef());
194  const Vec3 fOTmp(R1*fO);
195  const Vec3 fATmp(R1*fA);
196  const Vec3 fBTmp(R2*fB);
197  const Vec3 fITmp(R2*fI);
198 
199  const Vec3& Omega1(pNode1->GetWRef());
200  const Vec3& Omega2(pNode2->GetWRef());
201 
202  /* Force and slopes */
203  doublereal dF = GetF();
204  doublereal dFDE = GetFDE();
205  doublereal dFDEPrime = GetFDEPrime();
206 
207  const Vec3 b1 = pNode1->GetXCurr() + R1*fO;
208  const Vec3 b2 = pNode1->GetXCurr() + R1*fA;
209  const Vec3 b3 = pNode2->GetXCurr() + R2*fB;
210  const Vec3 b4 = pNode2->GetXCurr() + R2*fI;
211 
212  Vec3 p;
213  doublereal q, up, pNorm, dKC, dAp1, dAp2, dAp3, dAp4;
214  doublereal s = 1.0/(2.0*iIntSeg);
215 
216  Vec3 kcx1(Zero3);
217  Vec3 kcg1(Zero3);
218  Vec3 kcx2(Zero3);
219  Vec3 kcg2(Zero3);
220 
221  Vec3 TmpV;
222 
223  for (unsigned int jj = 0; jj < iIntSeg; jj++) {
224  q = s*(2*jj + 1);
225 
226  PntWght gp = gdi->GetFirst();
227  do {
228  up = s*gp.dGetPnt() + q;
229 
230  dAp1 = Ap1(up);
231  dAp2 = Ap2(up);
232  dAp3 = Ap3(up);
233  dAp4 = Ap4(up);
234 
235  p = b1*dAp1 + b2*dAp2 + b3*dAp3 + b4*dAp4;
236  pNorm = p.Norm();
237  dKC = (s*gp.dGetWght())/(2*dL0*pNorm);
238 
239  kcx1 = kcx1 + p*(dFDE*dKC*dCoef*(dAp1 + dAp2));
240 
241  kcg1 = kcg1 + p.Cross(fOTmp*dAp1 + fATmp*dAp2)*dFDE*dKC*dCoef;
242 
243  kcx2 = kcx2 + p*dFDE*dKC*dCoef*(dAp3 + dAp4);
244 
245  kcg2 = kcg2 + p.Cross(fBTmp*dAp3 +fITmp*dAp4)*dFDE*dKC*dCoef;
246 
247  if (dFDEPrime != 0.) {
248  kcx1 = kcx1 + p*dFDEPrime*dKC*(dAp1 + dAp2);
249 
250  TmpV = p*2. - p.Cross(fOTmp*dAp1 + fATmp*dAp2);
251  kcg1 = kcg1 - Omega1.Cross(TmpV)*dFDEPrime*dKC*dCoef;
252 
253  kcx2 = kcx2 + p*dFDEPrime*dKC*(dAp3 + dAp4);
254 
255  TmpV = p*2. - p.Cross(fBTmp*dAp3 + fITmp*dAp4);
256  kcg2 = kcg2 + Omega2.Cross(TmpV)*dFDEPrime*dKC*dCoef;
257  }
258 
259  } while (gdi->fGetNext(gp));
260  }
261 
262 
263  /* Forces on node 1 from dx1 */
264  Mat3x3 F1x1 = l1.Tens(kcx1);
265  WM.Add(1, 1, F1x1);
266 
267  /* Moments on node 1 from dx1 */
268  Mat3x3 M1x1 = Mat3x3(MatCross, fOTmp)*F1x1;
269  WM.Sub(3 + 1, 1, M1x1);
270 
271  /* Forces on node 2 from dx1 */
272  Mat3x3 F2x1 = l2.Tens(kcx1);
273  WM.Add(6 + 1, 1, F2x1);
274 
275  /* Moments on node 2 from dx1 */
276  Mat3x3 M2x1 = Mat3x3(MatCross, fITmp)*F2x1;
277  WM.Sub(9 + 1, 1, M2x1);
278 
279  /* Forces on node 1 from dg1 */
280  Mat3x3 F1g1 = l1.Tens(kcg1) - (Mat3x3(MatCross, l1) - Mat3x3(MatCross, l1)*l1.Tens())*dCoef*dF;
281  WM.Add(1, 3 + 1, F1g1);
282 
283  /* Moments on node 1 from dg1 */
284  Mat3x3 M1g1 = Mat3x3(MatCross, fOTmp)*F1g1 - Mat3x3(MatCrossCross, l1, fOTmp)*dCoef*dF;
285  WM.Sub(3 + 1, 3 + 1, M1g1);
286 
287  /* Forces on node 2 from dg1 */
288  Mat3x3 F2g1 = l2.Tens(kcg1);
289  WM.Add(6 + 1, 3 + 1, F2g1);
290 
291  /* Moments on node 2 from dg1 */
292  Mat3x3 M2g1 = Mat3x3(MatCross, fITmp)*F2g1;
293  WM.Sub(9 + 1, 3 + 1, M2g1);
294 
295  /* Forces on node 1 from dx2 */
296  Mat3x3 F1x2 = l1.Tens(kcx2);
297  WM.Add(1, 6 + 1, F1x2);
298 
299  /* Moments on node 1 from dx2 */
300  Mat3x3 M1x2 = Mat3x3(MatCross, fOTmp)*F1x2;
301  WM.Sub(3 + 1, 6 + 1, M1x2);
302 
303  /* Forces on node 2 from dx2 */
304  Mat3x3 F2x2 = l2.Tens(kcx2);
305  WM.Add(6 + 1, 6 + 1, F2x2);
306 
307  /* Moments on node 2 from dx2 */
308  Mat3x3 M2x2 = Mat3x3(MatCross, fITmp)*F2x2;
309  WM.Sub(9 + 1, 6 + 1, M2x2);
310 
311  /* Forces on node 1 from dg2 */
312  Mat3x3 F1g2 = l1.Tens(kcg2);
313  WM.Add(1, 9 + 1, F1g2);
314 
315  /* Moments on node 1 from dg2 */
316  Mat3x3 M1g2 = Mat3x3(MatCross, fOTmp)*F1g2;
317  WM.Sub(3 + 1, 9 + 1, M1g2);
318 
319  /* Forces on node 2 from dg2 */
320  Mat3x3 F2g2 = l2.Tens(kcg2) - (Mat3x3(MatCross, l2) - Mat3x3(MatCross, l2)*l2.Tens())*dF*dCoef;
321  WM.Add(6 + 1, 9 + 1, F2g2);
322 
323  /* Moments on node 2 from dg2 */
324  Mat3x3 M2g2 = Mat3x3(MatCross, fITmp)*F2g2 - Mat3x3(MatCrossCross, l2, fITmp)*dCoef*dF;
325  WM.Sub(9 + 1, 9 + 1, M2g2);
326 
327  return WorkMat;
328 }
329 
330 /* Residual contribute */
331 SubVectorHandler&
332 RodBezier::AssRes(SubVectorHandler& WorkVec,
333  doublereal /* dCoef */ ,
334  const VectorHandler& /* XCurr */ ,
335  const VectorHandler& /* XPrimeCurr */ )
336 {
337  DEBUGCOUT("RodBezier::AssRes()" << std::endl);
338 
339  /* Resizes and resets the work matrix */
340  integer iNumRows = 0;
341  integer iNumCols = 0;
342  WorkSpaceDim(&iNumRows, &iNumCols);
343  WorkVec.ResizeReset(iNumRows);
344 
345  /* Gets the nodes indexes */
346  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
347  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
348 
349  /* Sets the indexes */
350  for (int iCnt = 1; iCnt <= 6; iCnt++) {
351  WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex + iCnt);
352  WorkVec.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
353  }
354 
355  AssVec(WorkVec);
356 
357  return WorkVec;
358 }
359 
360 void
361 RodBezier::AssVec(SubVectorHandler& WorkVec)
362 {
363  DEBUGCOUT("RodBezier::AssVec()" << std::endl);
364 
365  /* Data */
366  const Mat3x3& R1(pNode1->GetRCurr());
367  const Mat3x3& R2(pNode2->GetRCurr());
368  Vec3 fOTmp(R1*fO);
369  Vec3 fATmp(R1*fA);
370  Vec3 fBTmp(R2*fB);
371  Vec3 fITmp(R2*fI);
372 
373  const Vec3& x1(pNode1->GetXCurr());
374  const Vec3& x2(pNode2->GetXCurr());
375 
376  const Vec3& v1(pNode1->GetVCurr());
377  const Vec3& v2(pNode2->GetVCurr());
378  const Vec3& Omega1(pNode1->GetWCurr());
379  const Vec3& Omega2(pNode2->GetWCurr());
380 
381  dElle = 0.;
382  doublereal dEllePrime = 0.;
383  Vec3 p, pprime;
384  doublereal q, up, dAp1, dAp2, dAp3, dAp4;
385  doublereal s = 1.0/(2.0*iIntSeg);
386 
387  for (unsigned int jj = 0; jj < iIntSeg; jj++) {
388  q = s*(2*jj + 1);
389 
390  PntWght gp = gdi->GetFirst();
391  do {
392  up = s*gp.dGetPnt() + q;
393 
394  dAp1 = Ap1(up);
395  dAp2 = Ap2(up);
396  dAp3 = Ap3(up);
397  dAp4 = Ap4(up);
398 
399  p = (x1 + fOTmp)*dAp1 +
400  (x1 + fATmp)*dAp2 +
401  (x2 + fBTmp)*dAp3 +
402  (x2 + fITmp)*dAp4;
403 
404  dElle = dElle + s*gp.dGetWght()*p.Norm();
405  pprime = (v1 + Omega1.Cross(fOTmp))*dAp1 +
406  (v1 + Omega1.Cross(fATmp))*dAp2 +
407  (v2 + Omega2.Cross(fBTmp))*dAp3 +
408  (v2 + Omega2.Cross(fITmp))*dAp4;
409  dEllePrime = dEllePrime + .5*s*gp.dGetWght()/p.Norm()*p.Dot(pprime);
410  } while (gdi->fGetNext(gp));
411  }
412 
413  /* Check that the distance is not null */
414  if (dElle <= std::numeric_limits<doublereal>::epsilon()) {
415  silent_cerr("RodBezier(" << GetLabel() << "): "
416  "null length of element." << std::endl);
417  throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
418  }
419 
420  /* Strain */
421  dEpsilon = dElle/dL0 - 1.;
422 
423  /* Strain velocity */
424  dEpsilonPrime = dEllePrime/dL0;
425 
426  /* Amplitude of force from CL */
427  bool ChangeJac(false);
428  try {
429  ConstitutiveLaw1DOwner::Update(dEpsilon, dEpsilonPrime);
430 
431  } catch (Elem::ChangedEquationStructure) {
432  ChangeJac = true;
433  }
434 
435  doublereal dF = GetF();
436 
437  /* Force vectors */
438  Vec3 F1(l1*dF);
439  Vec3 F2(l2*dF);
440 
441  WorkVec.Add(1, F1);
442  WorkVec.Add(3 + 1, fOTmp.Cross(F1));
443  WorkVec.Add(6 + 1, F2);
444  WorkVec.Add(9 + 1, fITmp.Cross(F2));
445 
446  if (ChangeJac) {
447  throw Elem::ChangedEquationStructure(MBDYN_EXCEPT_ARGS);
448  }
449 }
450 
451 void
452 RodBezier::Output(OutputHandler& OH) const
453 {
454  if (bToBeOutput()) {
455  ASSERT(dElle > std::numeric_limits<doublereal>::epsilon());
456  doublereal dF = GetF();
457 
458  std::ostream& out = OH.Joints();
459 
460  Joint::Output(out, "RodBezier", GetLabel(),
461  Vec3(dF, 0., 0.), Zero3, l1*dF, Zero3)
462  << " " << l2*dF << " " << dElle << " " << l1 << " "
463  << " " << l2 << " " << " " << dEpsilonPrime*dL0,
464  ConstitutiveLaw1DOwner::OutputAppend(out) << std::endl;
465  }
466 }
467 
468 /* Jacobian contribute during initial assembly */
469 VariableSubMatrixHandler&
470 RodBezier::InitialAssJac(VariableSubMatrixHandler& WorkMat,
471  const VectorHandler& /* XCurr */ )
472 {
473  DEBUGCOUT("Entering RodBezier::InitialAssJac()" << std::endl);
474 
475  FullSubMatrixHandler& WM = WorkMat.SetFull();
476 
477  /* Dimensiona e resetta la matrice di lavoro */
478  integer iNumRows = 0;
479  integer iNumCols = 0;
480  InitialWorkSpaceDim(&iNumRows, &iNumCols);
481  WM.ResizeReset(iNumRows, iNumCols);
482 
483  /* Recupera gli indici */
484  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
485  integer iNode1FirstVelIndex = iNode1FirstPosIndex + 6;
486  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
487  integer iNode2FirstVelIndex = iNode2FirstPosIndex + 6;
488 
489  /* Setta gli indici della matrice */
490  for (int iCnt = 1; iCnt <= 6; iCnt++) {
491  WM.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
492  WM.PutColIndex(iCnt, iNode1FirstPosIndex + iCnt);
493  WM.PutColIndex(6 + iCnt, iNode1FirstVelIndex + iCnt);
494 
495  WM.PutRowIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
496  WM.PutColIndex(12 + iCnt, iNode2FirstPosIndex + iCnt);
497  WM.PutColIndex(18 + iCnt, iNode2FirstVelIndex + iCnt);
498  }
499 
500  const Mat3x3& R1(pNode1->GetRRef());
501  const Mat3x3& R2(pNode2->GetRRef());
502  const Vec3 fOTmp(R1*fO);
503  const Vec3 fATmp(R1*fA);
504  const Vec3 fBTmp(R2*fB);
505  const Vec3 fITmp(R2*fI);
506 
507  const Vec3& x1(pNode1->GetXCurr());
508  const Vec3& x2(pNode2->GetXCurr());
509 
510  //const Vec3& v1(pNode1->GetVCurr());
511  //const Vec3& v2(pNode2->GetVCurr());
512  const Vec3& Omega1(pNode1->GetWRef());
513  const Vec3& Omega2(pNode2->GetWRef());
514 
515  /* Forza e slopes */
516  doublereal dF = GetF();
517  doublereal dFDE = GetFDE();
518  doublereal dFDEPrime = GetFDEPrime();
519 
520  const Vec3 b1 = x1 + R1*fO;
521  const Vec3 b2 = x1 + R1*fA;
522  const Vec3 b3 = x2 + R2*fB;
523  const Vec3 b4 = x2 + R2*fI;
524 
525  Vec3 p;
526  doublereal q, up, pNorm, dKC, dAp1, dAp2, dAp3, dAp4;
527  doublereal s = 1.0/(2.0*iIntSeg);
528 
529  Vec3 kx1(Zero3);
530  Vec3 kg1(Zero3);
531  Vec3 kx2(Zero3);
532  Vec3 kg2(Zero3);
533 
534  Vec3 cg1(Zero3);
535  Vec3 cxp1(Zero3);
536  Vec3 com1(Zero3);
537  Vec3 cg2(Zero3);
538  Vec3 cxp2(Zero3);
539  Vec3 com2(Zero3);
540 
541  Vec3 TmpV;
542 
543  for (unsigned int jj = 0; jj < iIntSeg; jj++) {
544  q = s*(2*jj + 1);
545 
546  PntWght gp = gdi->GetFirst();
547  do {
548  up = s*gp.dGetPnt() + q;
549 
550  dAp1 = Ap1(up);
551  dAp2 = Ap2(up);
552  dAp3 = Ap3(up);
553  dAp4 = Ap4(up);
554 
555  p = b1*dAp1 + b2*dAp2 + b3*dAp3 + b4*dAp4;
556  pNorm = p.Norm();
557  dKC = (s*gp.dGetWght())/(2*dL0*pNorm);
558 
559  kx1 = kx1 + p*dFDE*dKC*(dAp1 + dAp2);
560 
561  TmpV = fOTmp*dAp1 + fATmp*dAp2;
562  kg1 = kg1 - TmpV.Cross(p)*dFDE*dKC;
563 
564  kx2 = kx2 + p*dFDE*dKC*(dAp3 + dAp4);
565 
566  TmpV = fBTmp*dAp3 + fITmp*dAp4;
567  kg2 = kg2 - TmpV.Cross(p)*dFDE*dKC;
568 
569  if (dFDEPrime != 0.) {
570  TmpV = Omega1.Cross(p);
571  cg1 = cg1 - TmpV.Cross(fATmp*dAp2 - fOTmp*dAp1)*dFDEPrime*dKC;
572 
573  cxp1 = cxp1 + p*dFDEPrime*dKC*(dAp1 + dAp2);
574 
575  com1 = com1 - p.Cross(fATmp*dAp2 - fOTmp*dAp1)*dFDEPrime*dKC;
576 
577  TmpV = Omega2.Cross(p);
578  cg2 = cg2 + TmpV.Cross(fITmp*dAp4 - fBTmp*dAp3)*dFDEPrime*dKC;
579 
580  cxp2 = cxp2 + p*dFDEPrime*dKC*(dAp3 + dAp4);
581 
582  com2 = com2 - p.Cross(fITmp*dAp4 - fBTmp*dAp3)*dFDEPrime*dKC;
583 
584  }
585 
586  } while (gdi->fGetNext(gp));
587  }
588 
589  /* Force on node 1 from dx1 */
590  Mat3x3 F1x1 = l1.Tens(kx1);
591  WM.Add(1, 1, F1x1);
592 
593  /* Force on node 1 from dg1 */
594  Vec3 Tmp1 = kg1;
595  if (dFDEPrime != 0.) {
596  Tmp1 += cg1;
597  }
598  Mat3x3 F1g1 = l1.Tens(Tmp1) - (Mat3x3(MatCross, l1) - Mat3x3(MatCross, l1)*l1.Tens())*dF;
599  WM.Add(1, 3 + 1, F1g1);
600 
601  /* Force on node 1 from dxp1 */
602  Mat3x3 F1xp1;
603  if (dFDEPrime != 0.) {
604  F1xp1 = l1.Tens(cxp1);
605  WM.Add(1, 6 + 1, F1xp1);
606  }
607 
608  /* Force on node 1 from dom1 */
609  Mat3x3 F1om1;
610  if (dFDEPrime != 0.) {
611  F1om1 = l1.Tens(com1);
612  WM.Add(1, 9 + 1, F1om1);
613  }
614 
615  /* Force on node 1 from dx2 */
616  Mat3x3 F1x2 = l1.Tens(kx2);
617  WM.Add(1, 12 + 1, F1x2);
618 
619  /* Force on node 1 from dg2 */
620  Vec3 Tmp2 = kg2;
621  if (dFDEPrime != 0.) {
622  Tmp2 += cg2;
623  }
624  Mat3x3 F1g2 = l1.Tens(Tmp2);
625  WM.Add(1, 15 + 1, F1g2);
626 
627  /* Force on node 1 from dxp2 */
628  Mat3x3 F1xp2;
629  if (dFDEPrime != 0.) {
630  F1xp2 = l1.Tens(cxp2);
631  WM.Add(1, 18 + 1, F1xp2);
632  }
633 
634  /* Force on node 1 from dom2 */
635  Mat3x3 F1om2;
636  if (dFDEPrime != 0.) {
637  F1om2 = l1.Tens(com2);
638  WM.Add(1, 21 + 1, F1om2);
639  }
640 
641  /* Moment on node 1 from dx1 */
642  Mat3x3 M1x1 = Mat3x3(MatCross, fOTmp)*F1x1;
643  WM.Sub(3 + 1, 1, M1x1);
644 
645  /* Moment on node 1 from dg1 */
646  Mat3x3 M1g1 = Mat3x3(MatCross, fOTmp)*F1g1 + Mat3x3(MatCrossCross, l1, fOTmp);
647  WM.Sub(3 + 1, 3 + 1, M1g1);
648 
649  /* Moment on node 1 from dxp1 */
650  if (dFDEPrime != 0.) {
651  Mat3x3 M1xp1 = Mat3x3(MatCross, fOTmp)*F1xp1;
652  WM.Sub(3 + 1, 6 + 1, M1xp1);
653  }
654 
655  /* Moment on node 1 from dom1 */
656  if (dFDEPrime != 0.) {
657  Mat3x3 M1om1 = Mat3x3(MatCross, fOTmp)*F1om1;
658  WM.Sub(3 + 1, 9 + 1, M1om1);
659  }
660 
661  /* Moment on node 1 from dx2 */
662  Mat3x3 M1x2 = Mat3x3(MatCross, fOTmp)*F1x2;
663  WM.Sub(3 + 1, 12 + 1, M1x2);
664 
665  /* Moment on node 1 from dg2 */
666  Mat3x3 M1g2 = Mat3x3(MatCross, fOTmp)*F1g2;
667  WM.Sub(3 + 1, 15 + 1, M1g2);
668 
669  /* Moment on node 1 from dxp2 */
670  if (dFDEPrime != 0.) {
671  Mat3x3 M1xp2 = Mat3x3(MatCross, fOTmp)*F1xp2;
672  WM.Sub(3 + 1, 18 + 1, M1xp2);
673  }
674 
675  /* Moment on node 1 from dom2 */
676  if (dFDEPrime != 0.) {
677  Mat3x3 M1om2 = Mat3x3(MatCross, fOTmp)*F1om2;
678  WM.Sub(3 + 1, 21 + 1, M1om2);
679  }
680 
681  /* Force on node 2 from dx1 */
682  Mat3x3 F2x1 = l2.Tens(kx1);
683  WM.Add(6 + 1, 1, F1x1);
684 
685  /* Force on node 2 from dg1 */
686  Tmp1 = kg1;
687  Mat3x3 F2g1 = l2.Tens(Tmp1);
688  WM.Add(6 + 1, 3 + 1, F2g1);
689 
690  /* Force on node 2 from dxp1 */
691  Mat3x3 F2xp1;
692  if (dFDEPrime != 0.) {
693  F2xp1 = l2.Tens(cxp1);
694  WM.Add(6 + 1, 6 + 1, F2xp1);
695  }
696 
697  /* Force on node 2 from dom1 */
698  Mat3x3 F2om1;
699  if (dFDEPrime != 0.) {
700  F2om1 = l2.Tens(com1);
701  WM.Add(6 + 1, 9 + 1, F2om1);
702  }
703 
704  /* Force on node 2 from dx2 */
705  Mat3x3 F2x2 = l2.Tens(kx2);
706  WM.Add(6 + 1, 12 + 1, F2x2);
707 
708  /* Force on node 2 from dg2 */
709 
710  Mat3x3 F2g2 = l2.Tens(Tmp2) - (Mat3x3(MatCross, l2) - Mat3x3(MatCross, l2)*l2.Tens())*dF;
711  WM.Add(6 + 1, 15 + 1, F2g2);
712 
713  /* Force on node 2 from dxp2 */
714  Mat3x3 F2xp2;
715  if (dFDEPrime != 0.) {
716  F2xp2 = l2.Tens(cxp2);
717  WM.Add(6 + 1, 18 + 1, F2xp2);
718  }
719 
720  /* Force on node 2 from dom2 */
721  Mat3x3 F2om2;
722  if (dFDEPrime != 0.) {
723  F2om2 = l2.Tens(com2);
724  WM.Add(6 + 1, 21 + 1, F2om2);
725  }
726 
727  /* Moment on node 2 from dx2 */
728  Mat3x3 M2x1 = Mat3x3(MatCross, fITmp)*F2x1;
729  WM.Sub(9 + 1, 1, M2x1);
730 
731  /* Moment on node 2 from dg1 */
732  Mat3x3 M2g1 = Mat3x3(MatCross, fITmp)*F2g1;
733  WM.Sub(9 + 1, 3 + 1, M1g1);
734 
735  /* Moment on node 2 from dxp1 */
736  if (dFDEPrime != 0.) {
737  Mat3x3 M2xp1 = Mat3x3(MatCross, fITmp)*F2xp1;
738  WM.Sub(9 + 1, 6 + 1, M2xp1);
739  }
740 
741  /* Moment on node 2 from dom1 */
742  if (dFDEPrime != 0.) {
743  Mat3x3 M2om1 = Mat3x3(MatCross, fITmp)*F2om1;
744  WM.Sub(9 + 1, 9 + 1, M2om1);
745  }
746 
747  /* Moment on node 2 from dx2 */
748  Mat3x3 M2x2 = Mat3x3(MatCross, fITmp)*F2x2;
749  WM.Sub(9 + 1, 12 + 1, M2x2);
750 
751  /* Moment on node 2 from dg2 */
752  Mat3x3 M2g2 = Mat3x3(MatCross, fITmp)*F2g2 + Mat3x3(MatCrossCross, l2, fITmp);
753  WM.Sub(9 + 1, 15 + 1, M2g2);
754 
755  /* Moment on node 2 from dxp2 */
756  if (dFDEPrime != 0.) {
757  Mat3x3 M2xp2 = Mat3x3(MatCross, fITmp)*F2xp2;
758  WM.Sub(9 + 1, 18 + 1, M2xp2);
759  }
760 
761  /* Moment on node 2 from dom2 */
762  if (dFDEPrime != 0.) {
763  Mat3x3 M2om2 = Mat3x3(MatCross, fITmp)*F2om2;
764  WM.Sub(9 + 1, 21 + 1, M2om2);
765  }
766 
767  return WorkMat;
768 }
769 
770 /* Residual contribute during initial assembly */
771 SubVectorHandler&
772 RodBezier::InitialAssRes(SubVectorHandler& WorkVec,
773  const VectorHandler& /* XCurr */ )
774 {
775  DEBUGCOUT("RodBezier::InitialAssRes()" << std::endl);
776 
777  /* Dimensiona e resetta la matrice di lavoro */
778  integer iNumRows = 0;
779  integer iNumCols = 0;
780  InitialWorkSpaceDim(&iNumRows, &iNumCols);
781  WorkVec.ResizeReset(iNumRows);
782 
783  /* Indici */
784  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
785  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
786 
787  /* Setta gli indici */
788  for (int iCnt = 1; iCnt <= 6; iCnt++) {
789  WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex + iCnt);
790  WorkVec.PutRowIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
791  }
792 
793  AssVec(WorkVec);
794 
795  return WorkVec;
796 }
797 
798 /* Inverse dynamics capable element */
799 bool
800 RodBezier::bInverseDynamics(void) const
801 {
802  return true;
803 }
804 
805 /* Inverse Dynamics Jacobian matrix assembly */
806 VariableSubMatrixHandler&
807 RodBezier::AssJac(VariableSubMatrixHandler& WorkMat,
808  const VectorHandler& XCurr)
809 {
810  ASSERT(bIsErgonomy());
811  return AssJac(WorkMat, 1., XCurr, XCurr);
812 }
813 
814 /* Inverse Dynamics Residual Assembly */
815 SubVectorHandler&
816 RodBezier::AssRes(SubVectorHandler& WorkVec,
817  const VectorHandler& XCurr,
818  const VectorHandler& XPrimeCurr,
819  const VectorHandler& /* XPrimePrimeCurr */ ,
820  InverseDynamics::Order iOrder)
821 {
822  DEBUGCOUT("Entering RodBezier::AssRes()" << std::endl);
823 
824  ASSERT(iOrder == InverseDynamics::INVERSE_DYNAMICS
825  || (iOrder == InverseDynamics::POSITION && bIsErgonomy()));
826 
827  return AssRes(WorkVec, 1., XCurr, XPrimeCurr);
828 }
829 
830 /* Inverse Dyamics update */
831 void
832 RodBezier::Update(const VectorHandler& XCurr, InverseDynamics::Order iOrder)
833 {
834  NO_OP;
835 }
836 
837 /* Inverse Dynamics after convergence */
838 void
839 RodBezier::AfterConvergence(const VectorHandler& X,
840  const VectorHandler& XP, const VectorHandler& XPP)
841 {
842  AfterConvergence(X, XP);
843 }
844 
845 unsigned int
846 RodBezier::iGetNumPrivData(void) const
847 {
848  return 3 + ConstitutiveLaw1DOwner::iGetNumPrivData();
849 }
850 
851 unsigned int
852 RodBezier::iGetPrivDataIdx(const char *s) const
853 {
854  ASSERT(s != NULL);
855 
856  if (strcmp(s, "F") == 0) {
857  return 1;
858  }
859 
860  if (strcmp(s, "L") == 0) {
861  return 2;
862  }
863 
864  if (strcmp(s, "LPrime") == 0) {
865  return 3;
866  }
867 
868  size_t l = STRLENOF("constitutiveLaw.");
869  if (strncmp(s, "constitutiveLaw.", l) == 0) {
870  return 3 + ConstitutiveLaw1DOwner::iGetPrivDataIdx(&s[l]);
871  }
872 
873  /* error; handle later */
874  return 0;
875 }
876 
877 doublereal
878 RodBezier::dGetPrivData(unsigned int i) const
879 {
880  ASSERT(i > 0);
881 
882  switch (i) {
883  case 1:
884  return GetF();
885 
886  case 2:
887  return dElle;
888 
889  case 3:
890  return dL0*dEpsilonPrime;
891  }
892 
893  i -= 3;
894  ASSERT(i <= ConstitutiveLaw1DOwner::iGetNumPrivData());
895 
896  return ConstitutiveLaw1DOwner::dGetPrivData(i);
897 }
898 
899 /* RodBezier - end */
900 
RodBezier::RodBezier
RodBezier(unsigned int uL, const DofOwner *pDO, const ConstitutiveLaw1D *pCL, const StructNode *pN1, const StructNode *pN2, const Vec3 &fOTmp, const Vec3 &fATmp, const Vec3 &fBTmp, const Vec3 &fITmp, doublereal dLength, bool bFromNodes, const unsigned int iIntOrder, const unsigned int iIntSegments, flag fOut)
Definition: rodbezj.cc:52
RodBezier::AssVec
void AssVec(SubVectorHandler &WorkVec)
Definition: rodbezj.cc:361
RodBezier::~RodBezier
virtual ~RodBezier(void)
Definition: rodbezj.cc:130
FullSubMatrixHandler::PutColIndex
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
RodBezier::fI
const Vec3 fI
Definition: rodbezj.h:56
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
ConstitutiveLawOwner::pGetConstLaw
ConstitutiveLaw< T, Tder > * pGetConstLaw(void) const
Definition: constltp.h:278
MBDYN_EXCEPT_ARGS
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
Vec3
Definition: matvec3.h:98
RodBezier::l2
Vec3 l2
Definition: rodbezj.h:60
VectorHandler::ResizeReset
virtual void ResizeReset(integer)
Definition: vh.cc:55
MatCross
const MatCross_Manip MatCross
Definition: matvec3.cc:639
RodBezier::iIntOrd
unsigned int iIntOrd
Definition: rodbezj.h:65
VariableSubMatrixHandler::SetFull
FullSubMatrixHandler & SetFull(void)
Definition: submat.h:1168
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
StructDispNode::GetNodeType
virtual Node::Type GetNodeType(void) const
Definition: strnode.cc:145
Vec3::Norm
doublereal Norm(void) const
Definition: matvec3.h:263
RodBezier::fB
const Vec3 fB
Definition: rodbezj.h:55
Vec3::Dot
doublereal Dot(const Vec3 &v) const
Definition: matvec3.h:243
Elem::bIsErgonomy
bool bIsErgonomy(void) const
Definition: elem.cc:83
FullSubMatrixHandler::Add
void Add(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:209
ConstitutiveLawOwner::iGetPrivDataIdx
virtual unsigned int iGetPrivDataIdx(const char *s) const
Definition: constltp.h:361
RodBezier::bInverseDynamics
virtual bool bInverseDynamics(void) const
Definition: rodbezj.cc:800
GaussDataIterator::fGetNext
flag fGetNext(doublereal &d, integer i=0) const
Definition: gauss.cc:205
GaussDataIterator::GetFirst
PntWght GetFirst(void) const
Definition: gauss.cc:198
RodBezier::AfterConvergence
virtual void AfterConvergence(const VectorHandler &X, const VectorHandler &XP)
Definition: rodbezj.cc:155
RodBezier::AssRes
virtual SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: rodbezj.cc:332
ConstitutiveLawOwner::GetFDE
const Tder & GetFDE(void) const
Definition: constltp.h:298
RodBezier::Ap2
doublereal Ap2(doublereal u)
Definition: rodbezj.h:182
ConstitutiveLawOwner::iGetNumPrivData
virtual unsigned int iGetNumPrivData(void) const
Definition: constltp.h:352
StructNode::GetWRef
virtual const Vec3 & GetWRef(void) const
Definition: strnode.h:1024
NO_OP
#define NO_OP
Definition: myassert.h:74
RodBezier::Output
void Output(OutputHandler &OH) const
Definition: rodbezj.cc:452
RodBezier::InitialAssJac
virtual VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: rodbezj.cc:470
RodBezier::gdi
GaussDataIterator * gdi
Definition: rodbezj.h:68
SubVectorHandler::PutRowIndex
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
RodBezier::Restart
virtual std::ostream & Restart(std::ostream &out) const
Definition: rodbezj.cc:137
ConstitutiveLawOwner::OutputAppend
virtual std::ostream & OutputAppend(std::ostream &out) const
Definition: constltp.h:373
RodBezier::iIntSeg
unsigned int iIntSeg
Definition: rodbezj.h:66
RodBezier::fA
const Vec3 fA
Definition: rodbezj.h:54
RodBezier::InitialWorkSpaceDim
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: rodbezj.h:126
DEBUGCOUT
#define DEBUGCOUT(msg)
Definition: myassert.h:232
RodBezier::l1
Vec3 l1
Definition: rodbezj.h:59
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
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
RodBezier::Ap4
doublereal Ap4(doublereal u)
Definition: rodbezj.h:184
ConstitutiveLawOwner::AfterConvergence
void AfterConvergence(const T &Eps, const T &EpsPrime=mb_zero< T >())
Definition: constltp.h:288
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
Vec3::Tens
Mat3x3 Tens(const Vec3 &v) const
Definition: matvec3.cc:53
RodBezier::iGetPrivDataIdx
virtual unsigned int iGetPrivDataIdx(const char *s) const
Definition: rodbezj.cc:852
RodBezier::dElle
doublereal dElle
Definition: rodbezj.h:61
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
FullSubMatrixHandler::ResizeReset
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
RodBezier::pNode1
const StructNode * pNode1
Definition: rodbezj.h:51
ConstitutiveLawOwner::dGetPrivData
virtual doublereal dGetPrivData(unsigned int i) const
Definition: constltp.h:369
MatCrossCross
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
STRLENOF
#define STRLENOF(s)
Definition: mbdyn.h:166
Elem
Definition: elem.h:75
PntWght::dGetPnt
doublereal dGetPnt(void) const
Definition: gauss.h:55
RodBezier::Ap3
doublereal Ap3(doublereal u)
Definition: rodbezj.h:183
FullSubMatrixHandler::PutRowIndex
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
ConstitutiveLawOwner::GetF
const T & GetF(void) const
Definition: constltp.h:293
RodBezier::InitialAssRes
virtual SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: rodbezj.cc:772
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
RodBezier::fO
const Vec3 fO
Definition: rodbezj.h:53
doublereal
double doublereal
Definition: colamd.c:52
ConstitutiveLawOwner::GetFDEPrime
const Tder & GetFDEPrime(void) const
Definition: constltp.h:303
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
integer
long int integer
Definition: colamd.c:51
ConstitutiveLawOwner::Update
void Update(const T &Eps, const T &EpsPrime=mb_zero< T >())
Definition: constltp.h:283
RodBezier::WorkSpaceDim
virtual void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: rodbezj.h:100
RodBezier::Update
void Update(const VectorHandler &XCurr, InverseDynamics::Order iOrder=InverseDynamics::INVERSE_DYNAMICS)
Definition: rodbezj.cc:832
RodBezier::dEpsilon
doublereal dEpsilon
Definition: rodbezj.h:62
WithLabel::GetLabel
unsigned int GetLabel(void) const
Definition: withlab.cc:62
RodBezier::pNode2
const StructNode * pNode2
Definition: rodbezj.h:52
RodBezier::dL0
doublereal dL0
Definition: rodbezj.h:57
RodBezier::dGetPrivData
doublereal dGetPrivData(unsigned int i) const
Definition: rodbezj.cc:878
RodBezier::AssJac
virtual VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: rodbezj.cc:163
RodBezier::iGetNumPrivData
virtual unsigned int iGetNumPrivData(void) const
Definition: rodbezj.cc:846
RodBezier::dEpsilonPrime
doublereal dEpsilonPrime
Definition: rodbezj.h:63
RodBezier::Ap1
doublereal Ap1(doublereal u)
Definition: rodbezj.h:181
PntWght
Definition: gauss.h:50