MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
module-autodiff_test.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/modules/module-autodiff_test/module-autodiff_test.cc,v 1.6 2017/01/12 14:47:35 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 /*
33  AUTHOR: Reinhard Resch <r.resch@secop.com>
34  Copyright (C) 2013(-2017) all rights reserved.
35 
36  The copyright of this code is transferred
37  to Pierangelo Masarati and Paolo Mantegazza
38  for use in the software MBDyn as described
39  in the GNU Public License version 2.1
40 */
41 
42 #define CREATE_PROFILE 1
43 
44 #include <limits>
45 #include <iostream>
46 #include <iomanip>
47 #include <cfloat>
48 #include <cassert>
49 #include <cmath>
50 #include <cstring>
51 #include <ctime>
52 
53 #ifdef HAVE_CONFIG_H
54 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
55 #endif /* HAVE_CONFIG_H */
56 
57 #include <dataman.h>
58 #include <userelem.h>
59 
60 #include "module-autodiff_test.h"
61 #ifdef USE_AUTODIFF
62 
63 #include <clock_time.h>
64 #include <gradient.h>
65 #include <matvec.h>
66 #include <matvecass.h>
67 #include <ac/f2c.h>
68 
69 extern "C" int __FC_DECL__(def_joint_ad_ass_res)(const doublereal X1[3],
70  const doublereal g1[3],
71  const doublereal XP1[3],
72  const doublereal gP1[3],
73  const doublereal R1_0[3*3],
74  const doublereal W1_0[3],
75  const doublereal X2[2],
76  const doublereal g2[3],
77  const doublereal XP2[3],
78  const doublereal gP2[3],
79  const doublereal R2_0[3*3],
80  const doublereal W2_0[3],
81  const doublereal S1[3*3],
82  const doublereal D1[3*3],
83  const doublereal o1[3],
84  const doublereal o2[3],
85  doublereal F1[3],
86  doublereal M1[3],
87  doublereal F2[3],
88  doublereal M2[3]);
89 
90 extern "C" int __FC_DECL__(def_joint_ad_ass_res_dv)(const doublereal X1[3],
91  const doublereal X1d[3][12],
92  const doublereal g1[3],
93  const doublereal g1d[3][12],
94  const doublereal XP1[3],
95  const doublereal XP1d[3][12],
96  const doublereal gP1[3],
97  const doublereal gP1d[3][12],
98  const doublereal R1_0[3*3],
99  const doublereal W1_0[3],
100  const doublereal X2[3],
101  const doublereal X2d[3][12],
102  const doublereal g2[3],
103  const doublereal g2d[3][12],
104  const doublereal XP2[3],
105  const doublereal XP2d[3][12],
106  const doublereal gP2[3],
107  const doublereal gp2d[3][12],
108  const doublereal R2_0[3*3],
109  const doublereal W2_0[3*3],
110  const doublereal S1[3*3],
111  const doublereal D1[3*3],
112  const doublereal o1[3],
113  const doublereal o2[3],
114  doublereal F1[3],
115  doublereal F1d[3][12],
116  doublereal M1[3],
117  doublereal M1d[3][12],
118  doublereal F2[3],
119  doublereal F2d[3][12],
120  doublereal M2[3],
121  doublereal M2d[3][12],
122  const integer &nbdirs);
123 
124 using namespace grad;
125 
126 class DeformableJointAD: virtual public Elem, public UserDefinedElem
127 {
128 public:
129  DeformableJointAD(unsigned uLabel, const DofOwner *pDO,
130  DataManager* pDM, MBDynParser& HP);
131  virtual ~DeformableJointAD(void);
132  virtual void Output(OutputHandler& OH) const;
133  virtual void WorkSpaceDim(integer* piNumRows, integer* piNumCols) const;
135  AssJac(VariableSubMatrixHandler& WorkMat,
136  doublereal dCoef,
137  const VectorHandler& XCurr,
138  const VectorHandler& XPrimeCurr);
140  AssRes(SubVectorHandler& WorkVec,
141  doublereal dCoef,
142  const VectorHandler& XCurr,
143  const VectorHandler& XPrimeCurr);
145  AssJacTrad(VariableSubMatrixHandler& WorkMat,
146  doublereal dCoef,
147  const VectorHandler& XCurr,
148  const VectorHandler& XPrimeCurr);
150  AssResTrad(SubVectorHandler& WorkVec,
151  doublereal dCoef,
152  const VectorHandler& XCurr,
153  const VectorHandler& XPrimeCurr);
155  AssJacF77(VariableSubMatrixHandler& WorkMat,
156  doublereal dCoef,
157  const VectorHandler& XCurr,
158  const VectorHandler& XPrimeCurr);
160  AssResF77(SubVectorHandler& WorkVec,
161  doublereal dCoef,
162  const VectorHandler& XCurr,
163  const VectorHandler& XPrimeCurr);
164  template <typename T>
165  inline void
166  AssRes(GradientAssVec<T>& WorkVec,
167  doublereal dCoef,
168  const GradientVectorHandler<T>& XCurr,
169  const GradientVectorHandler<T>& XPrimeCurr,
170  enum FunctionCall func);
171  unsigned int iGetNumPrivData(void) const;
172  int iGetNumConnectedNodes(void) const;
173  void GetConnectedNodes(std::vector<const Node *>& connectedNodes) const;
174  void SetValue(DataManager *pDM, VectorHandler& X, VectorHandler& XP,
176  std::ostream& Restart(std::ostream& out) const;
177  virtual unsigned int iGetInitialNumDof(void) const;
178  virtual void
179  InitialWorkSpaceDim(integer* piNumRows, integer* piNumCols) const;
181  InitialAssJac(VariableSubMatrixHandler& WorkMat,
182  const VectorHandler& XCurr);
184  InitialAssRes(SubVectorHandler& WorkVec, const VectorHandler& XCurr);
185 
186  private:
187  static const index_type iNumADVars = 12;
188  const StructNode* pNode1;
189  const StructNode* pNode2;
190  //Matrix<doublereal, 3, 3> S1;
191  //Matrix<doublereal, 3, 3> D1;
192  //Vector<doublereal, 3> o1;
193  //Vector<doublereal, 3> o2;
194  Mat3x3 S1;
195  Mat3x3 D1;
196  Vec3 o1;
197  Vec3 o2;
199 
200  enum AssemblyFlag {
201  TRADITIONAL = 0,
202  TEMPLATE_META_PROG = 1,
203  F77 = 2
204  } fRes, fJac;
205 
206 #if CREATE_PROFILE == 1
207  enum {
208  RESIDUAL = 0,
209  JACOBIAN = 1
210  };
211  struct {
212  doublereal dtRes;
213  doublereal dtJac;
214  doublereal dtInit[2];
215  doublereal dtGet[2];
216  doublereal dtCalc[2];
217  doublereal dtAss[2];
218  } profile;
219 
220  static const char* AssemblyFuncName(enum AssemblyFlag flag);
221 #endif
222 };
223 
224 DeformableJointAD::DeformableJointAD(
225  unsigned uLabel, const DofOwner *pDO,
226  DataManager* pDM, MBDynParser& HP)
227 : Elem(uLabel, flag(0)),
228  UserDefinedElem(uLabel, pDO),
229  pNode1(0),
230  pNode2(0),
231  fRes(TRADITIONAL),
232  fJac(TRADITIONAL)
233 {
234 #if CREATE_PROFILE == 1
235  memset(&profile, 0, sizeof(profile));
236 #endif
237 
238  // help
239  if (HP.IsKeyWord("help"))
240  {
241  silent_cout(
242  "\n"
243  "Module: DeformableJointAD\n"
244  "\n"
245  " deformable joint ad,\n"
246  " node1, (label) <node1>,\n"
247  " [offset, (Vec3) <o1>,]\n"
248  " node2, (label) <node2>,\n"
249  " [offset, (Vec3) <o2>,]\n"
250  " stiffness, (Mat3x3) S1,\n"
251  " damping, (Mat3x3) D1,\n"
252  << std::endl);
253 
254  if (!HP.IsArg())
255  {
256  /*
257  * Exit quietly if nothing else is provided
258  */
259  throw NoErr(MBDYN_EXCEPT_ARGS);
260  }
261  }
262 
263  if (!HP.IsKeyWord("node1"))
264  {
265  silent_cerr("deformable joint ad" << GetLabel() << "): keyword \"node1\" expected at line " << HP.GetLineData() << std::endl);
267  }
268 
269  pNode1 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
270 
271  if (!pNode1) {
272  silent_cerr("deformable joint ad" << GetLabel() << "): structural node expected at line " << HP.GetLineData() << std::endl);
274  }
275 
276  if (HP.IsKeyWord("offset")) {
277  o1 = HP.GetPosRel(ReferenceFrame(pNode1));
278  }
279 
280  if (!HP.IsKeyWord("node2"))
281  {
282  silent_cerr("deformable joint ad" << GetLabel() << "): keyword \"node2\" expected at line " << HP.GetLineData() << std::endl);
284  }
285 
286  pNode2 = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
287 
288  if (!pNode2) {
289  silent_cerr("deformable joint ad" << GetLabel() << "): structural node expected at line " << HP.GetLineData() << std::endl);
291  }
292 
293  if ( HP.IsKeyWord("offset") ) {
294  o2 = HP.GetPosRel(ReferenceFrame(pNode2));
295  }
296 
297  if (!HP.IsKeyWord("stiffness")) {
298  silent_cerr("deformable joint ad" << GetLabel() << "): keyword \"stiffness\" expected at line " << HP.GetLineData() << std::endl);
300  }
301 
302  S1 = HP.GetMat3x3();
303 
304  if (!HP.IsKeyWord("damping")) {
305  silent_cerr("deformable joint ad" << GetLabel() << "): keyword \"damping\" expected at line " << HP.GetLineData() << std::endl);
307  }
308 
309  D1 = HP.GetMat3x3();
310 
311  if (HP.IsKeyWord("residual")) {
312  if (HP.IsKeyWord("traditional")) {
313  fRes = TRADITIONAL;
314  } else if (HP.IsKeyWord("template" "meta" "program")) {
315  fRes = TEMPLATE_META_PROG;
316  } else if (HP.IsKeyWord("fortran77")) {
317  fRes = F77;
318  } else {
319  silent_cerr("deformable joint ad" << GetLabel() << "): keyword \"traditional\" or \"template meta program\" expected at line " << HP.GetLineData() << std::endl);
321  }
322  }
323 
324  if (HP.IsKeyWord("jacobian")) {
325  if (HP.IsKeyWord("traditional")) {
326  fJac = TRADITIONAL;
327  } else if (HP.IsKeyWord("template" "meta" "program")) {
328  fJac = TEMPLATE_META_PROG;
329  } else if (HP.IsKeyWord("fortran77")) {
330  fJac = F77;
331  } else {
332  silent_cerr("deformable joint ad" << GetLabel() << "): keyword \"traditional\" or \"template meta program\" expected at line " << HP.GetLineData() << std::endl);
334  }
335  }
336 
337  SetOutputFlag(pDM->fReadOutput(HP, Elem::LOADABLE));
338 }
339 
340 DeformableJointAD::~DeformableJointAD(void)
341 {
342 #if CREATE_PROFILE == 1
343  std::cerr << "deformable joint ad(" << GetLabel() << ")" << std::endl;
344 
345  std::cerr << "dtRes["
346  << AssemblyFuncName(fRes)
347  << "]=" << profile.dtRes << std::endl;
348 
349  std::cerr << "dtJac["
350  << AssemblyFuncName(fJac)
351  << "]=" << profile.dtJac << std::endl;
352 
353 
354  std::cerr << "dtInit[RESIDUAL]=" << profile.dtInit[RESIDUAL] << std::endl;
355  std::cerr << "dtGet[RESIDUAL]=" << profile.dtGet[RESIDUAL] << std::endl;
356  std::cerr << "dtCalc[RESIDUAL]=" << profile.dtCalc[RESIDUAL] << std::endl;
357  std::cerr << "dtAss[RESIDUAL]=" << profile.dtAss[RESIDUAL] << std::endl;
358 
359  std::cerr << "dtInit[JACOBIAN]=" << profile.dtInit[JACOBIAN] << std::endl;
360  std::cerr << "dtGet[JACOBIAN]=" << profile.dtGet[JACOBIAN] << std::endl;
361  std::cerr << "dtCalc[JACOBIAN]=" << profile.dtCalc[JACOBIAN] << std::endl;
362  std::cerr << "dtAss[JACOBIAN]=" << profile.dtAss[JACOBIAN] << std::endl;
363 
364  std::cerr << std::endl;
365 #endif
366 }
367 
368 void
369 DeformableJointAD::Output(OutputHandler& OH) const
370 {
371 
372 }
373 
374 void
375 DeformableJointAD::WorkSpaceDim(integer* piNumRows, integer* piNumCols) const
376 {
377  *piNumRows = 12;
378  *piNumCols = 12;
379 }
380 
382 DeformableJointAD::AssJac(VariableSubMatrixHandler& WorkMat,
383  doublereal dCoef,
384  const VectorHandler& XCurr,
385  const VectorHandler& XPrimeCurr)
386 {
387 #if CREATE_PROFILE == 1
388  const double start = mbdyn_clock_time();
389 #endif
390 
391  switch (fJac) {
392  case TRADITIONAL:
393  AssJacTrad(WorkMat, dCoef, XCurr, XPrimeCurr);
394  break;
395 
396  case TEMPLATE_META_PROG:
398  WorkMat.SetSparse(),
399  dCoef,
400  XCurr,
401  XPrimeCurr,
402  REGULAR_JAC,
403  &dof);
404  break;
405 
406  case F77:
407  AssJacF77(WorkMat, dCoef, XCurr, XPrimeCurr);
408  break;
409 
410  default:
411  ASSERT(false);
412  }
413 
414 #if CREATE_PROFILE == 1
415  profile.dtJac += mbdyn_clock_time() - start;
416 #endif
417 
418  return WorkMat;
419 }
420 
422 DeformableJointAD::AssRes(SubVectorHandler& WorkVec,
423  doublereal dCoef,
424  const VectorHandler& XCurr,
425  const VectorHandler& XPrimeCurr)
426 {
427 #if CREATE_PROFILE == 1
428  const double start = mbdyn_clock_time();
429 #endif
430 
431  switch (fRes) {
432  case TRADITIONAL:
433  AssResTrad(WorkVec, dCoef, XCurr, XPrimeCurr);
434  break;
435 
436  case TEMPLATE_META_PROG:
438  WorkVec,
439  dCoef,
440  XCurr,
441  XPrimeCurr,
442  REGULAR_RES);
443  break;
444 
445  case F77:
446  AssResF77(WorkVec, dCoef, XCurr, XPrimeCurr);
447  break;
448 
449  default:
450  ASSERT(false);
451  }
452 
453 #if CREATE_PROFILE == 1
454  profile.dtRes += mbdyn_clock_time() - start;
455 #endif
456 
457  return WorkVec;
458 }
459 
461 DeformableJointAD::AssJacTrad(VariableSubMatrixHandler& WorkMatVar,
462  doublereal dCoef,
463  const VectorHandler& XCurr,
464  const VectorHandler& XPrimeCurr)
465 {
466 #if CREATE_PROFILE == 1
467  doublereal start = mbdyn_clock_time();
468 #endif
469 
470  const Vec3& X1 = pNode1->GetXCurr();
471  const Vec3& V1 = pNode1->GetVCurr();
472  const Mat3x3& R1 = pNode1->GetRCurr();
473  const Mat3x3& R1_0 = pNode1->GetRRef();
474  const Vec3& W1 = pNode1->GetWCurr();
475  const Vec3& W1_0 = pNode1->GetWRef();
476 
477  const Vec3& X2 = pNode2->GetXCurr();
478  const Vec3& V2 = pNode2->GetVCurr();
479  const Mat3x3& R2 = pNode2->GetRCurr();
480  const Mat3x3& R2_0 = pNode2->GetRRef();
481  const Vec3& W2 = pNode2->GetWCurr();
482  const Vec3& W2_0 = pNode2->GetWRef();
483 
484 #if CREATE_PROFILE == 1
485  profile.dtGet[JACOBIAN] += mbdyn_clock_time() - start;
486  start = mbdyn_clock_time();
487 #endif
488 
489  const Vec3 R1o1 = R1 * o1;
490  const Mat3x3 skew_R1o1(MatCross, R1o1);
491  const Vec3 R1_0o1 = R1_0 * o1;
492  const Mat3x3 skew_R1_0o1(MatCross, R1_0o1);
493  const Vec3 R2o2 = R2 * o2;
494  const Mat3x3 skew_R2o2(MatCross, R2o2);
495  const Vec3 R2_0o2 = R2_0 * o2;
496  const Mat3x3 skew_R2_0o2(MatCross, R2_0o2);
497  const Vec3 dX = R1.MulTV(X1 + R1o1 - X2 - R2o2);
498  const Vec3 dV = R1.MulTV(V1 + W1.Cross(R1o1) - V2 - W2.Cross(R2o2));
499  const Vec3 F1_R1 = -(S1 * dX + D1 * dV);
500  const Vec3 F1 = R1 * F1_R1;
501  const Vec3 F2 = -F1;
502 
503  const Mat3x3 dF1_dX1 = -R1 * S1.MulMT(R1);
504 
505  const Mat3x3 ddX_dg1 = R1_0.MulTM(Mat3x3(MatCross, X1 + R1o1 - X2 - R2o2)) - R1.MulTM(skew_R1_0o1);
506  const Mat3x3 ddV_dg1 = R1_0.MulTM(Mat3x3(MatCross, V1 + W1.Cross(R1o1) - V2 - W2.Cross(R2o2)))
507  + R1.MulTM(Mat3x3(MatCrossCross, R1o1, W1_0) - Mat3x3(MatCrossCross, W1, R1_0o1));
508 
509  const Mat3x3 dF1_dg1 = Mat3x3(MatCross, R1_0 * (-F1_R1)) - R1 * (S1 * ddX_dg1 + D1 * ddV_dg1);
510 
511  const Mat3x3 dF1_dX2 = R1 * S1.MulMT(R1);
512  const Mat3x3 ddX_dg2 = R1.MulTM(skew_R2_0o2);
513  const Mat3x3 ddV_dg2 = R1.MulTM(Mat3x3(MatCrossCross, R2o2, -W2_0) + Mat3x3(MatCrossCross, W2, R2_0o2));
514  const Mat3x3 dF1_dg2 = -R1 * (S1 * ddX_dg2 + D1 * ddV_dg2);
515 
516  const Mat3x3 dF2_dX1 = -dF1_dX1;
517  const Mat3x3 dF2_dg1 = -dF1_dg1;
518  const Mat3x3 dF2_dX2 = -dF1_dX2;
519  const Mat3x3 dF2_dg2 = -dF1_dg2;
520 
521  const Mat3x3 dM1_dX1 = skew_R1o1 * dF1_dX1;
522  const Mat3x3 dM1_dg1 = Mat3x3(MatCrossCross, F1, R1_0o1) + skew_R1o1 * dF1_dg1;
523  const Mat3x3 dM1_dX2 = skew_R1o1 * dF1_dX2;
524  const Mat3x3 dM1_dg2 = skew_R1o1 * dF1_dg2;
525 
526  const Mat3x3 dM2_dX1 = skew_R2o2 * dF2_dX1;
527  const Mat3x3 dM2_dg1 = skew_R2o2 * dF2_dg1;
528  const Mat3x3 dM2_dX2 = skew_R2o2 * dF2_dX2;
529  const Mat3x3 dM2_dg2 = Mat3x3(MatCrossCross, F2, R2_0o2) + skew_R2o2 * dF2_dg2;
530 
531  const Mat3x3 dF1_dV1 = -R1 * D1.MulMT(R1);
532  const Mat3x3 ddV_dgP1 = -R1.MulTM(skew_R1o1);
533  const Mat3x3 dF1_dgP1 = -R1 * D1 * ddV_dgP1;
534 
535  const Mat3x3 dF1_dV2 = R1 * D1.MulMT(R1);
536  const Mat3x3 ddV_dgP2 = R1.MulTM(skew_R2o2);
537  const Mat3x3 dF1_dgP2 = -R1 * D1 * ddV_dgP2;
538 
539  const Mat3x3 dM1_dV1 = skew_R1o1 * dF1_dV1;
540  const Mat3x3 dM1_dgP1 = skew_R1o1 * dF1_dgP1;
541  const Mat3x3 dM1_dV2 = skew_R1o1 * dF1_dV2;
542  const Mat3x3 dM1_dgP2 = skew_R1o1 * dF1_dgP2;
543 
544  const Mat3x3 dF2_dV1 = -dF1_dV1;
545  const Mat3x3 dF2_dgP1 = -dF1_dgP1;
546  const Mat3x3 dF2_dV2 = -dF1_dV2;
547  const Mat3x3 dF2_dgP2 = -dF1_dgP2;
548 
549  const Mat3x3 dM2_dV1 = skew_R2o2 * dF2_dV1;
550  const Mat3x3 dM2_dgP1 = skew_R2o2 * dF2_dgP1;
551  const Mat3x3 dM2_dV2 = skew_R2o2 * dF2_dV2;
552  const Mat3x3 dM2_dgP2 = skew_R2o2 * dF2_dgP2;
553 
554 #if CREATE_PROFILE == 1
555  profile.dtCalc[JACOBIAN] += mbdyn_clock_time() - start;
556  start = mbdyn_clock_time();
557 #endif
558 
559  FullSubMatrixHandler& WorkMat = WorkMatVar.SetFull();
560 
561  integer iNumRows, iNumCols;
562 
563  WorkSpaceDim(&iNumRows, &iNumCols);
564 
565  WorkMat.ResizeReset(iNumRows, iNumCols);
566 
567  const integer iFirstMomIndexNode1 = pNode1->iGetFirstMomentumIndex();
568  const integer iFirstMomIndexNode2 = pNode2->iGetFirstMomentumIndex();
569  const integer iFirstPosIndexNode1 = pNode1->iGetFirstPositionIndex();
570  const integer iFirstPosIndexNode2 = pNode2->iGetFirstPositionIndex();
571 
572  for (integer i = 1; i <= 6; ++i) {
573  WorkMat.PutRowIndex(i, iFirstMomIndexNode1 + i);
574  WorkMat.PutRowIndex(i + 6, iFirstMomIndexNode2 + i);
575  WorkMat.PutColIndex(i, iFirstPosIndexNode1 + i);
576  WorkMat.PutColIndex(i + 6, iFirstPosIndexNode2 + i);
577  }
578 
579  WorkMat.Put(1, 1, -dF1_dV1 - dF1_dX1 * dCoef);
580  WorkMat.Put(1, 4, -dF1_dgP1 - dF1_dg1 * dCoef);
581  WorkMat.Put(1, 7, -dF1_dV2 - dF1_dX2 * dCoef);
582  WorkMat.Put(1, 10, -dF1_dgP2 - dF1_dg2 * dCoef);
583 
584  WorkMat.Put(4, 1, -dM1_dV1 - dM1_dX1 * dCoef);
585  WorkMat.Put(4, 4, -dM1_dgP1 - dM1_dg1 * dCoef);
586  WorkMat.Put(4, 7, -dM1_dV2 - dM1_dX2 * dCoef);
587  WorkMat.Put(4, 10, -dM1_dgP2 - dM1_dg2 * dCoef);
588 
589  WorkMat.Put(7, 1, -dF2_dV1 - dF2_dX1 * dCoef);
590  WorkMat.Put(7, 4, -dF2_dgP1 - dF2_dg1 * dCoef);
591  WorkMat.Put(7, 7, -dF2_dV2 - dF2_dX2 * dCoef);
592  WorkMat.Put(7, 10, -dF2_dgP2 - dF2_dg2 * dCoef);
593 
594  WorkMat.Put(10, 1, -dM2_dV1 - dM2_dX1 * dCoef);
595  WorkMat.Put(10, 4, -dM2_dgP1 - dM2_dg1 * dCoef);
596  WorkMat.Put(10, 7, -dM2_dV2 - dM2_dX2 * dCoef);
597  WorkMat.Put(10, 10, -dM2_dgP2 - dM2_dg2 * dCoef);
598 
599 #if CREATE_PROFILE == 1
600  profile.dtAss[JACOBIAN] += mbdyn_clock_time() - start;
601 #endif
602 
603  return WorkMatVar;
604 }
605 
607 DeformableJointAD::AssResTrad(SubVectorHandler& WorkVec,
608  doublereal dCoef,
609  const VectorHandler& XCurr,
610  const VectorHandler& XPrimeCurr)
611 {
612 #if CREATE_PROFILE == 1
613  doublereal start = mbdyn_clock_time();
614 #endif
615 
616  const Vec3& X1 = pNode1->GetXCurr();
617  const Vec3& XP1 = pNode1->GetVCurr();
618  const Mat3x3& R1 = pNode1->GetRCurr();
619  const Vec3& W1 = pNode1->GetWCurr();
620 
621  const Vec3& X2 = pNode2->GetXCurr();
622  const Vec3& XP2 = pNode2->GetVCurr();
623  const Mat3x3& R2 = pNode2->GetRCurr();
624  const Vec3& W2 = pNode2->GetWCurr();
625 
626 #if CREATE_PROFILE == 1
627  profile.dtGet[RESIDUAL] += mbdyn_clock_time() - start;
628  start = mbdyn_clock_time();
629 #endif
630 
631  const Vec3 R1o1 = R1 * o1;
632  const Vec3 R2o2 = R2 * o2;
633  const Vec3 dX = R1.MulTV(X1 + R1o1 - X2 - R2o2);
634  const Vec3 dXP = R1.MulTV(XP1 + W1.Cross(R1o1) - XP2 - W2.Cross(R2o2));
635  const Vec3 F1 = -(R1 * (S1 * dX + D1 * dXP));
636  const Vec3 F2 = -F1;
637  const Vec3 M1 = R1o1.Cross(F1);
638  const Vec3 M2 = R2o2.Cross(F2);
639 
640 #if CREATE_PROFILE == 1
641  profile.dtCalc[RESIDUAL] += mbdyn_clock_time() - start;
642  start = mbdyn_clock_time();
643 #endif
644 
645  const integer iFirstMomIndexNode1 = pNode1->iGetFirstMomentumIndex();
646  const integer iFirstMomIndexNode2 = pNode2->iGetFirstMomentumIndex();
647 
648  integer iNumRows, iNumCols;
649 
650  WorkSpaceDim(&iNumRows, &iNumCols);
651 
652  WorkVec.ResizeReset(iNumRows);
653 
654  for (integer i = 1; i <= 6; ++i) {
655  WorkVec.PutRowIndex(i, iFirstMomIndexNode1 + i);
656  WorkVec.PutRowIndex(i + 6, iFirstMomIndexNode2 + i);
657  }
658 
659  WorkVec.Put(1, F1);
660  WorkVec.Put(4, M1);
661  WorkVec.Put(7, F2);
662  WorkVec.Put(10, M2);
663 
664 #if CREATE_PROFILE == 1
665  profile.dtAss[RESIDUAL] += mbdyn_clock_time() - start;
666 #endif
667 
668  return WorkVec;
669 }
670 
672 DeformableJointAD::AssJacF77(VariableSubMatrixHandler& WorkMatVar,
673  doublereal dCoef,
674  const VectorHandler& XCurr,
675  const VectorHandler& XPrimeCurr)
676 {
677 #if CREATE_PROFILE == 1
678  doublereal start = mbdyn_clock_time();
679 #endif
680 
681  const doublereal* X1 = pNode1->GetXCurr().pGetVec();
682  const doublereal* g1 = pNode1->GetgCurr().pGetVec();
683  const doublereal* XP1 = pNode1->GetVCurr().pGetVec();
684  const doublereal* gP1 = pNode1->GetgPCurr().pGetVec();
685  const doublereal* R1_0 = pNode1->GetRRef().pGetMat();
686  const doublereal* W1_0 = pNode1->GetWRef().pGetVec();
687  const doublereal* X2 = pNode2->GetXCurr().pGetVec();
688  const doublereal* g2 = pNode2->GetgCurr().pGetVec();
689  const doublereal* XP2 = pNode2->GetVCurr().pGetVec();
690  const doublereal* gP2 = pNode2->GetgPCurr().pGetVec();
691  const doublereal* R2_0 = pNode2->GetRRef().pGetMat();
692  const doublereal* W2_0 = pNode2->GetWRef().pGetVec();
693 
694  doublereal X1d[3][12] = {{0.}}, g1d[3][12] = {{0.}};
695  doublereal XP1d[3][12] = {{0.}}, gP1d[3][12] = {{0.}};
696  doublereal X2d[3][12] = {{0.}}, g2d[3][12] = {{0.}};
697  doublereal XP2d[3][12] = {{0.}}, gP2d[3][12] = {{0.}};
698 
699  for (integer i = 0; i < 3; ++i) {
700  X1d[i][i] = -dCoef;
701  XP1d[i][i] = -1.;
702  g1d[i][i + 3] = -dCoef;
703  gP1d[i][i + 3] = -1.;
704  X2d[i][i + 6] = -dCoef;
705  XP2d[i][i + 6] = -1.;
706  g2d[i][i + 9] = -dCoef;
707  gP2d[i][i + 9] = -1.;
708  }
709 
710 #if CREATE_PROFILE == 1
711  profile.dtGet[JACOBIAN] += mbdyn_clock_time() - start;
712  start = mbdyn_clock_time();
713 #endif
714 
715  doublereal F1[3], M1[3], F2[3], M2[3];
716  doublereal F1d[3][12], M1d[3][12], F2d[3][12], M2d[3][12];
717 
718  __FC_DECL__(def_joint_ad_ass_res_dv)(X1,
719  X1d,
720  g1,
721  g1d,
722  XP1,
723  XP1d,
724  gP1,
725  gP1d,
726  R1_0,
727  W1_0,
728  X2,
729  X2d,
730  g2,
731  g2d,
732  XP2,
733  XP2d,
734  gP2,
735  gP2d,
736  R2_0,
737  W2_0,
738  S1.pGetMat(),
739  D1.pGetMat(),
740  o1.pGetVec(),
741  o2.pGetVec(),
742  F1,
743  F1d,
744  M1,
745  M1d,
746  F2,
747  F2d,
748  M2,
749  M2d,
750  12);
751 
752 #if CREATE_PROFILE == 1
753  profile.dtCalc[JACOBIAN] += mbdyn_clock_time() - start;
754  start = mbdyn_clock_time();
755 #endif
756 
757  FullSubMatrixHandler& WorkMat = WorkMatVar.SetFull();
758 
759  integer iNumRows, iNumCols;
760 
761  WorkSpaceDim(&iNumRows, &iNumCols);
762 
763  WorkMat.ResizeReset(iNumRows, iNumCols);
764 
765  const integer iFirstMomIndexNode1 = pNode1->iGetFirstMomentumIndex();
766  const integer iFirstMomIndexNode2 = pNode2->iGetFirstMomentumIndex();
767  const integer iFirstPosIndexNode1 = pNode1->iGetFirstPositionIndex();
768  const integer iFirstPosIndexNode2 = pNode2->iGetFirstPositionIndex();
769 
770  for (integer i = 1; i <= 6; ++i) {
771  WorkMat.PutRowIndex(i, iFirstMomIndexNode1 + i);
772  WorkMat.PutRowIndex(i + 6, iFirstMomIndexNode2 + i);
773  WorkMat.PutColIndex(i, iFirstPosIndexNode1 + i);
774  WorkMat.PutColIndex(i + 6, iFirstPosIndexNode2 + i);
775  }
776 
777  for (integer i = 0; i < 3; ++i) {
778  for (integer j = 0; j < 12; ++j) {
779  WorkMat.PutCoef(i + 1, j + 1, F1d[i][j]);
780  WorkMat.PutCoef(i + 4, j + 1, M1d[i][j]);
781  WorkMat.PutCoef(i + 7, j + 1, F2d[i][j]);
782  WorkMat.PutCoef(i + 10, j + 1, M2d[i][j]);
783  }
784  }
785 
786 #if CREATE_PROFILE == 1
787  profile.dtAss[JACOBIAN] += mbdyn_clock_time() - start;
788 #endif
789 
790  return WorkMatVar;
791 }
792 
794 DeformableJointAD::AssResF77(SubVectorHandler& WorkVec,
795  doublereal dCoef,
796  const VectorHandler& XCurr,
797  const VectorHandler& XPrimeCurr)
798 {
799 #if CREATE_PROFILE == 1
800  doublereal start = mbdyn_clock_time();
801 #endif
802 
803  const doublereal* X1 = pNode1->GetXCurr().pGetVec();
804  const doublereal* g1 = pNode1->GetgCurr().pGetVec();
805  const doublereal* XP1 = pNode1->GetVCurr().pGetVec();
806  const doublereal* gP1 = pNode1->GetgPCurr().pGetVec();
807  const doublereal* R1_0 = pNode1->GetRRef().pGetMat();
808  const doublereal* W1_0 = pNode1->GetWRef().pGetVec();
809  const doublereal* X2 = pNode2->GetXCurr().pGetVec();
810  const doublereal* g2 = pNode2->GetgCurr().pGetVec();
811  const doublereal* XP2 = pNode2->GetVCurr().pGetVec();
812  const doublereal* gP2 = pNode2->GetgPCurr().pGetVec();
813  const doublereal* R2_0 = pNode2->GetRRef().pGetMat();
814  const doublereal* W2_0 = pNode2->GetWRef().pGetVec();
815 
816 #if CREATE_PROFILE == 1
817  profile.dtGet[RESIDUAL] += mbdyn_clock_time() - start;
818  start = mbdyn_clock_time();
819 #endif
820 
821  doublereal F1[3], M1[3], F2[3], M2[3];
822 
823  __FC_DECL__(def_joint_ad_ass_res)(X1,
824  g1,
825  XP1,
826  gP1,
827  R1_0,
828  W1_0,
829  X2,
830  g2,
831  XP2,
832  gP2,
833  R2_0,
834  W2_0,
835  S1.pGetMat(),
836  D1.pGetMat(),
837  o1.pGetVec(),
838  o2.pGetVec(),
839  F1,
840  M1,
841  F2,
842  M2);
843 
844 #if CREATE_PROFILE == 1
845  profile.dtCalc[RESIDUAL] += mbdyn_clock_time() - start;
846  start = mbdyn_clock_time();
847 #endif
848 
849  const integer iFirstMomIndexNode1 = pNode1->iGetFirstMomentumIndex();
850  const integer iFirstMomIndexNode2 = pNode2->iGetFirstMomentumIndex();
851 
852  integer iNumRows, iNumCols;
853 
854  WorkSpaceDim(&iNumRows, &iNumCols);
855 
856  WorkVec.ResizeReset(iNumRows);
857 
858  for (integer i = 1; i <= 6; ++i) {
859  WorkVec.PutRowIndex(i, iFirstMomIndexNode1 + i);
860  WorkVec.PutRowIndex(i + 6, iFirstMomIndexNode2 + i);
861  }
862 
863  for (integer i = 0; i < 3; ++i) {
864  WorkVec.PutCoef(1 + i, F1[i]);
865  WorkVec.PutCoef(4 + i, M1[i]);
866  WorkVec.PutCoef(7 + i, F2[i]);
867  WorkVec.PutCoef(10 + i, M2[i]);
868  }
869 
870 #if CREATE_PROFILE == 1
871  profile.dtAss[RESIDUAL] += mbdyn_clock_time() - start;
872 #endif
873 
874  return WorkVec;
875 }
876 
877 template <typename T>
878 inline void DeformableJointAD::AssRes(GradientAssVec<T>& WorkVec,
879  doublereal dCoef,
880  const GradientVectorHandler<T>& XCurr,
881  const GradientVectorHandler<T>& XPrimeCurr,
882  enum FunctionCall func)
883 {
884  typedef Matrix<T, 3, 3> Mat3x3;
885  typedef Vector<T, 3> Vec3;
886 
887 #if CREATE_PROFILE == 1
888  doublereal start = mbdyn_clock_time();
889 #endif
890 
891  Mat3x3 R1, R2;
892  Vec3 X1, X2, XP1, XP2, W1, W2;
893 
894 #if CREATE_PROFILE == 1
895  const index_type iFunc = (func == grad::REGULAR_RES) ? RESIDUAL : JACOBIAN;
896  profile.dtInit[iFunc] += mbdyn_clock_time() - start;
897  start = mbdyn_clock_time();
898 #endif
899 
900  pNode1->GetXCurr(X1, dCoef, func, &dof);
901  pNode1->GetVCurr(XP1, dCoef, func, &dof);
902  pNode1->GetRCurr(R1, dCoef, func, &dof);
903  pNode1->GetWCurr(W1, dCoef, func, &dof);
904 
905  pNode2->GetXCurr(X2, dCoef, func, &dof);
906  pNode2->GetVCurr(XP2, dCoef, func, &dof);
907  pNode2->GetRCurr(R2, dCoef, func, &dof);
908  pNode2->GetWCurr(W2, dCoef, func, &dof);
909 
910 #if CREATE_PROFILE == 1
911  profile.dtGet[iFunc] += mbdyn_clock_time() - start;
912  start = mbdyn_clock_time();
913 #endif
914 
915  const Vec3 R1o1 = R1 * o1;
916  const Vec3 R2o2 = R2 * o2;
917  const Vec3 dX = Transpose(R1) * Vec3(X1 + R1o1 - X2 - R2o2);
918 
919  const Vec3 dXP = Transpose(R1) * Vec3(XP1 + Cross(W1, R1o1) - XP2 - Cross(W2, R2o2));
920  const Vec3 F1 = -(R1 * Vec3(S1 * dX + D1 * dXP));
921 
922  const Vec3 F2 = -F1;
923  const Vec3 M1 = Cross(R1o1, F1);
924  const Vec3 M2 = Cross(R2o2, F2);
925 
926 #if CREATE_PROFILE == 1
927  profile.dtCalc[iFunc] += mbdyn_clock_time() - start;
928  start = mbdyn_clock_time();
929 #endif
930 
931  const integer iFirstMomIndexNode1 = pNode1->iGetFirstMomentumIndex();
932  const integer iFirstMomIndexNode2 = pNode2->iGetFirstMomentumIndex();
933 
934  WorkVec.AddItem(iFirstMomIndexNode1 + 1, F1);
935  WorkVec.AddItem(iFirstMomIndexNode1 + 4, M1);
936  WorkVec.AddItem(iFirstMomIndexNode2 + 1, F2);
937  WorkVec.AddItem(iFirstMomIndexNode2 + 4, M2);
938 
939 #if CREATE_PROFILE == 1
940  profile.dtAss[iFunc] += mbdyn_clock_time() - start;
941 #endif
942 }
943 
944 unsigned int
945 DeformableJointAD::iGetNumPrivData(void) const
946 {
947  return 0;
948 }
949 
950 int
951 DeformableJointAD::iGetNumConnectedNodes(void) const
952 {
953  return 2; // 1x shaft + 1x bearing
954 }
955 
956 void
957 DeformableJointAD::GetConnectedNodes(std::vector<const Node *>& connectedNodes) const
958 {
959  connectedNodes.resize(iGetNumConnectedNodes());
960  connectedNodes[0] = pNode1;
961  connectedNodes[1] = pNode2;
962 }
963 
964 void
965 DeformableJointAD::SetValue(DataManager *pDM,
968 {
969 
970 }
971 
972 std::ostream&
973 DeformableJointAD::Restart(std::ostream& out) const
974 {
975  return out;
976 }
977 
978 unsigned int
979 DeformableJointAD::iGetInitialNumDof(void) const
980 {
981  return 0;
982 }
983 
984 void
985 DeformableJointAD::InitialWorkSpaceDim(
986  integer* piNumRows,
987  integer* piNumCols) const
988 {
989  *piNumRows = 0;
990  *piNumCols = 0;
991 }
992 
994 DeformableJointAD::InitialAssJac(
995  VariableSubMatrixHandler& WorkMat,
996  const VectorHandler& XCurr)
997 {
998  WorkMat.SetNullMatrix();
999 
1000  return WorkMat;
1001 }
1002 
1004 DeformableJointAD::InitialAssRes(
1005  SubVectorHandler& WorkVec,
1006  const VectorHandler& XCurr)
1007 {
1008  WorkVec.ResizeReset(0);
1009 
1010  return WorkVec;
1011 }
1012 
1013 #if CREATE_PROFILE == 1
1014 const char* DeformableJointAD::AssemblyFuncName(enum AssemblyFlag flag)
1015 {
1016  switch (flag) {
1017  case TRADITIONAL:
1018  return "traditional";
1019 
1020  case TEMPLATE_META_PROG:
1021  return "template meta program";
1022 
1023  case F77:
1024  return "Fortran 77";
1025 
1026  default:
1027  return "?";
1028  }
1029 }
1030 #endif
1031 
1032 class InlineJointAD: virtual public Elem, public UserDefinedElem
1033 {
1034 public:
1035  InlineJointAD(unsigned uLabel, const DofOwner *pDO,
1036  DataManager* pDM, MBDynParser& HP);
1037  virtual ~InlineJointAD(void);
1038  virtual unsigned int iGetNumDof(void) const;
1039  virtual DofOrder::Order GetDofType(unsigned int i) const;
1040  virtual DofOrder::Order GetEqType(unsigned int i) const;
1041  virtual std::ostream& DescribeDof(std::ostream& out, const char *prefix, bool bInitial) const;
1042  virtual std::ostream& DescribeEq(std::ostream& out, const char *prefix, bool bInitial) const;
1043  virtual unsigned int iGetNumPrivData(void) const;
1044  virtual unsigned int iGetPrivDataIdx(const char *s) const;
1045  virtual doublereal dGetPrivData(unsigned int i) const;
1046  virtual void Output(OutputHandler& OH) const;
1047  virtual void WorkSpaceDim(integer* piNumRows, integer* piNumCols) const;
1050  doublereal dCoef,
1051  const VectorHandler& XCurr,
1052  const VectorHandler& XPrimeCurr);
1054  AssRes(SubVectorHandler& WorkVec,
1055  doublereal dCoef,
1056  const VectorHandler& XCurr,
1057  const VectorHandler& XPrimeCurr);
1058  template <typename T>
1059  inline void
1060  AssRes(GradientAssVec<T>& WorkVec,
1061  doublereal dCoef,
1062  const GradientVectorHandler<T>& XCurr,
1063  const GradientVectorHandler<T>& XPrimeCurr,
1064  enum FunctionCall func);
1065  int iGetNumConnectedNodes(void) const;
1066  void GetConnectedNodes(std::vector<const Node *>& connectedNodes) const;
1067  void SetValue(DataManager *pDM, VectorHandler& X, VectorHandler& XP,
1069  virtual void AfterPredict(VectorHandler& X, VectorHandler& XP);
1070  virtual void Update(const VectorHandler& XCurr,const VectorHandler& XPrimeCurr);
1071  std::ostream& Restart(std::ostream& out) const;
1072  virtual unsigned int iGetInitialNumDof(void) const;
1073  virtual void
1074  InitialWorkSpaceDim(integer* piNumRows, integer* piNumCols) const;
1077  const VectorHandler& XCurr);
1079  InitialAssRes(SubVectorHandler& WorkVec, const VectorHandler& XCurr);
1080  template <typename T>
1081  inline void
1083  const GradientVectorHandler<T>& XCurr,
1084  enum FunctionCall func);
1085 
1086  private:
1087  StructNode* pNode1;
1090  StructNode* pNode2;
1092 
1093  enum LagrangeMultiplierIndex
1094  {
1095  L1 = 0,
1096  L2 = 1
1097  };
1098 
1099  doublereal lambda[2];
1100  LocalDofMap dof;
1101 
1102  static const index_type iWorkSpace = 14;
1103  static const index_type iInitialWorkSpace = 28;
1104 };
1105 
1106 InlineJointAD::InlineJointAD(
1107  unsigned uLabel, const DofOwner *pDO,
1108  DataManager* pDM, MBDynParser& HP)
1109 : Elem(uLabel, flag(0)),
1110  UserDefinedElem(uLabel, pDO),
1111  pNode1(0),
1112  o1(Zero3),
1113  e(Eye3),
1114  pNode2(0),
1115  o2(Zero3)
1116 {
1117  memset(lambda, 0, sizeof(lambda));
1118 
1119  // help
1120  if (HP.IsKeyWord("help")) {
1121  silent_cout(
1122  "\n"
1123  "Module: InlineAD\n"
1124  "\n"
1125  " This element implements a inline joint with friction\n"
1126  "\n"
1127  " inline friction,\n"
1128  " node1, (label) <node1>,\n"
1129  " [ offset, (Vec3) <offset>, ]\n"
1130  " [ hinge, (Mat3x3) <orientation>, ]\n"
1131  " node2, (label) <node2>,\n"
1132  " [ offset, (Vec3) <offset>, ]\n"
1133  "\n"
1134  << std::endl);
1135 
1136  if (!HP.IsArg()) {
1137  /*
1138  * Exit quietly if nothing else is provided
1139  */
1140  throw NoErr(MBDYN_EXCEPT_ARGS);
1141  }
1142  }
1143 
1144  if ( !HP.IsKeyWord("node1") ) {
1145  silent_cerr("inline friction(" << GetLabel() << "): keyword \"node1\" expected at line " << HP.GetLineData() << std::endl);
1147  }
1148 
1149  pNode1 = dynamic_cast<StructNode*>(pDM->ReadNode(HP,Node::STRUCTURAL));
1150 
1151  if (!pNode1) {
1152  silent_cerr("inline friction ad(" << GetLabel() << "): structural node expected at line " << HP.GetLineData() << std::endl);
1154  }
1155 
1156  const ReferenceFrame refNode1(pNode1);
1157 
1158  if (HP.IsKeyWord("offset")) {
1159  o1 = HP.GetPosRel(refNode1);
1160  }
1161 
1162  if (HP.IsKeyWord("hinge") || HP.IsKeyWord("orientation")) {
1163  e = HP.GetRotRel(refNode1);
1164  }
1165 
1166  if (!HP.IsKeyWord("node2")) {
1167  silent_cerr("inline friction ad(" << GetLabel() << "): keyword \"node2\" expected at line " << HP.GetLineData() << std::endl);
1169  }
1170 
1171  pNode2 = dynamic_cast<StructNode*>(pDM->ReadNode(HP,Node::STRUCTURAL));
1172 
1173  if (!pNode2) {
1174  silent_cerr("inline friction ad(" << GetLabel() << "): structural node expected at line " << HP.GetLineData() << std::endl);
1176  }
1177 
1178  if (HP.IsKeyWord("offset")) {
1179  const ReferenceFrame refNode2(pNode2);
1180 
1181  o2 = HP.GetPosRel(refNode2);
1182  }
1183 
1184  SetOutputFlag(pDM->fReadOutput(HP, Elem::LOADABLE));
1185 
1186  std::ostream& out = pDM->GetLogFile();
1187 
1188  out << "inline joint ad: " << GetLabel() << " "
1189  << pNode1->GetLabel() << " "
1190  << o1 << " "
1191  << e << " "
1192  << pNode2->GetLabel() << " " << o2 << " "
1193  << std::endl;
1194 }
1195 
1196 InlineJointAD::~InlineJointAD(void)
1197 {
1198  // destroy private data
1199 }
1200 
1201 unsigned int InlineJointAD::iGetNumDof(void) const
1202 {
1203  return 2u;
1204 }
1205 
1206 DofOrder::Order InlineJointAD::GetDofType(unsigned int i) const
1207 {
1208  switch (i) {
1209  case 0:
1210  case 1:
1211  return DofOrder::ALGEBRAIC;
1212 
1213  default:
1214  ASSERT(0);
1216  }
1217 }
1218 
1219 DofOrder::Order InlineJointAD::GetEqType(unsigned int i) const
1220 {
1221  switch (i) {
1222  case 0:
1223  case 1:
1224  return DofOrder::ALGEBRAIC;
1225 
1226  default:
1227  ASSERT(0);
1229  }
1230 }
1231 
1232 std::ostream& InlineJointAD::DescribeDof(std::ostream& out, const char *prefix, bool bInitial) const
1233 {
1234  const integer iFirstIndex = iGetFirstIndex();
1235 
1236  out << prefix << iFirstIndex + 1 << "->" << iFirstIndex + 2 << ": reaction forces [lambda1, lambda2]" << std::endl;
1237 
1238  if (bInitial) {
1239  out << prefix << iFirstIndex + 3 << "->" << iFirstIndex + 4 << ": reaction force derivatives [lambdaP1, lambdaP2]" << std::endl;
1240  }
1241 
1242  return out;
1243 }
1244 
1245 std::ostream& InlineJointAD::DescribeEq(std::ostream& out, const char *prefix, bool bInitial) const
1246 {
1247  const integer iFirstIndex = iGetFirstIndex();
1248 
1249  out << prefix << iFirstIndex + 1 << "->" << iFirstIndex + 2 << ": position constraints [c1, c2]" << std::endl;
1250 
1251  if (bInitial) {
1252  out << prefix << iFirstIndex + 3 << "->" << iFirstIndex + 4 << ": velocity constraints [cP1, cP2]" << std::endl;
1253  }
1254 
1255  return out;
1256 }
1257 
1258 unsigned int InlineJointAD::iGetNumPrivData(void) const
1259 {
1260  return 2;
1261 }
1262 
1263 unsigned int InlineJointAD::iGetPrivDataIdx(const char *s) const
1264 {
1265  static const struct {
1266  int index;
1267  char name[8];
1268  } data[] = {
1269  { 1, "lambda1" },
1270  { 2, "lambda2" }
1271  };
1272 
1273  const int N = sizeof(data) / sizeof(data[0]);
1274 
1275  for (int i = 0; i < N; ++i) {
1276  if (0 == strcmp(data[i].name, s)) {
1277  return data[i].index;
1278  }
1279  }
1280 
1281  return 0;
1282 }
1283 
1284 doublereal InlineJointAD::dGetPrivData(unsigned int i) const
1285 {
1286  switch (i) {
1287  case 1:
1288  case 2:
1289  return lambda[i - 1];
1290 
1291  default:
1292  silent_cerr("inline ad(" << GetLabel() << "): invalid private data index " << i << std::endl);
1294  }
1295 }
1296 
1297 void
1298 InlineJointAD::Output(OutputHandler& OH) const
1299 {
1300  if ( bToBeOutput() )
1301  {
1302  if ( OH.UseText(OutputHandler::LOADABLE) )
1303  {
1304  std::ostream& os = OH.Loadable();
1305 
1306  os << std::setw(8) << GetLabel() << " " << lambda[L1] << " " << lambda[L2] << std::endl;
1307  }
1308  }
1309 }
1310 
1311 void
1312 InlineJointAD::WorkSpaceDim(integer* piNumRows, integer* piNumCols) const
1313 {
1314  *piNumRows = *piNumCols = iWorkSpace;
1315 }
1316 
1318 InlineJointAD::AssJac(VariableSubMatrixHandler& WorkMat,
1319  doublereal dCoef,
1320  const VectorHandler& XCurr,
1321  const VectorHandler& XPrimeCurr)
1322 {
1323  GradientAssVec<Gradient<iWorkSpace> >::AssJac(this,
1324  WorkMat.SetSparse(),
1325  dCoef,
1326  XCurr,
1327  XPrimeCurr,
1328  REGULAR_JAC,
1329  &dof);
1330 
1331  return WorkMat;
1332 }
1333 
1334 
1336 InlineJointAD::AssRes(SubVectorHandler& WorkVec,
1337  doublereal dCoef,
1338  const VectorHandler& XCurr,
1339  const VectorHandler& XPrimeCurr)
1340 {
1342  WorkVec,
1343  dCoef,
1344  XCurr,
1345  XPrimeCurr,
1346  REGULAR_RES);
1347 
1348  return WorkVec;
1349 }
1350 
1351 template <typename T>
1352 inline void
1353 InlineJointAD::AssRes(GradientAssVec<T>& WorkVec,
1354  doublereal dCoef,
1355  const GradientVectorHandler<T>& XCurr,
1356  const GradientVectorHandler<T>& XPrimeCurr,
1357  enum FunctionCall func) {
1358 
1359  typedef Vector<T, 3> Vec3;
1360  typedef Matrix<T, 3, 3> Mat3x3;
1361 
1362  const integer iFirstIndex = iGetFirstIndex();
1363  const integer iFirstMomentumIndexNode1 = pNode1->iGetFirstMomentumIndex();
1364  const integer iFirstMomentumIndexNode2 = pNode2->iGetFirstMomentumIndex();
1365 
1366  Vec3 X1, X2;
1367  Mat3x3 R1, R2;
1368 
1369  pNode1->GetXCurr(X1, dCoef, func, &dof);
1370  pNode1->GetRCurr(R1, dCoef, func, &dof);
1371  pNode2->GetXCurr(X2, dCoef, func, &dof);
1372  pNode2->GetRCurr(R2, dCoef, func, &dof);
1373 
1374  Vector<T, 2> lambda;
1375 
1376  XCurr.GetVec(iFirstIndex + 1, lambda, 1., &dof); // Note: for algebraic variables dCoef is always one
1377 
1378  const Vec3 R2o2 = R2 * o2;
1379  const Vec3 l1 = X2 + R2o2 - X1;
1380 
1381  const Vec3 F1 = R1 * Vec3(e.GetCol(2) * lambda(1) + e.GetCol(3) * lambda(2));
1382  const Vec3 M1 = Cross(l1, F1);
1383  const Vec3 F2 = -F1;
1384  const Vec3 M2 = Cross(R2o2, F2);
1385 
1386  const Vec3 a = Transpose(R1) * l1 - o1;
1387 
1388  WorkVec.AddItem(iFirstMomentumIndexNode1 + 1, F1);
1389  WorkVec.AddItem(iFirstMomentumIndexNode1 + 4, M1);
1390  WorkVec.AddItem(iFirstMomentumIndexNode2 + 1, F2);
1391  WorkVec.AddItem(iFirstMomentumIndexNode2 + 4, M2);
1392 
1393  for (integer i = 1; i <= 2; ++i) {
1394  WorkVec.AddItem(iFirstIndex + i, Dot(e.GetCol(i + 1), a) / dCoef);
1395  }
1396 }
1397 
1398 int
1399 InlineJointAD::iGetNumConnectedNodes(void) const
1400 {
1401  return 2;
1402 }
1403 
1404 void
1405 InlineJointAD::GetConnectedNodes(std::vector<const Node *>& connectedNodes) const
1406 {
1407  connectedNodes.resize(iGetNumConnectedNodes());
1408  connectedNodes[0] = pNode1;
1409  connectedNodes[1] = pNode2;
1410 }
1411 
1412 void
1413 InlineJointAD::SetValue(DataManager *pDM,
1414  VectorHandler& X, VectorHandler& XP,
1416 {
1417  const integer iFirstIndex = iGetFirstIndex();
1418 
1419  for (int i = 1; i <= 2; ++i) {
1420  X.PutCoef(iFirstIndex + i, lambda[i - 1]);
1421  }
1422 }
1423 
1424 void InlineJointAD::AfterPredict(VectorHandler& X, VectorHandler& XP)
1425 {
1426  Update(X, XP);
1427 }
1428 
1429 void InlineJointAD::Update(const VectorHandler& XCurr,const VectorHandler& XPrimeCurr)
1430 {
1431  const integer iFirstIndex = iGetFirstIndex();
1432 
1433  for (int i = 1; i <= 2; ++i) {
1434  lambda[i - 1] = XCurr(iFirstIndex + i);
1435  }
1436 }
1437 
1438 std::ostream&
1439 InlineJointAD::Restart(std::ostream& out) const
1440 {
1441  return out;
1442 }
1443 
1444 unsigned int
1445 InlineJointAD::iGetInitialNumDof(void) const
1446 {
1447  return 4u;
1448 }
1449 
1450 void
1451 InlineJointAD::InitialWorkSpaceDim(
1452  integer* piNumRows,
1453  integer* piNumCols) const
1454 {
1455  *piNumRows = *piNumCols = iInitialWorkSpace;
1456 }
1457 
1459 InlineJointAD::InitialAssJac(
1460  VariableSubMatrixHandler& WorkMat,
1461  const VectorHandler& XCurr)
1462 {
1463 
1464  GradientAssVec<Gradient<iInitialWorkSpace> >::InitialAssJac(this,
1465  WorkMat.SetSparse(),
1466  XCurr,
1468  &dof);
1469 
1470  return WorkMat;
1471 }
1472 
1474 InlineJointAD::InitialAssRes(
1475  SubVectorHandler& WorkVec,
1476  const VectorHandler& XCurr)
1477 {
1479  WorkVec,
1480  XCurr,
1481  INITIAL_ASS_RES);
1482 
1483  return WorkVec;
1484 }
1485 
1486 template <typename T>
1487 inline void
1488 InlineJointAD::InitialAssRes(GradientAssVec<T>& WorkVec,
1489  const GradientVectorHandler<T>& XCurr,
1490  enum FunctionCall func) {
1491 
1492  typedef Vector<T, 3> Vec3;
1493  typedef Matrix<T, 3, 3> Mat3x3;
1494 
1495  Vec3 X1, XP1, X2, XP2, omega1, omega2;
1496  Mat3x3 R1, R2;
1497 
1498  pNode1->GetXCurr(X1, 1., func, &dof); // Note: during initial assembly dCoef is always one
1499  pNode1->GetRCurr(R1, 1., func, &dof);
1500  pNode1->GetVCurr(XP1, 1., func, &dof);
1501  pNode1->GetWCurr(omega1, 1., func, &dof);
1502 
1503  pNode2->GetXCurr(X2, 1., func, &dof);
1504  pNode2->GetRCurr(R2, 1., func, &dof);
1505  pNode2->GetVCurr(XP2, 1., func, &dof);
1506  pNode2->GetWCurr(omega2, 1., func, &dof);
1507 
1508  const integer iFirstIndexNode1 = pNode1->iGetFirstIndex();
1509  const integer iFirstIndexNode2 = pNode2->iGetFirstIndex();
1510  const integer iFirstIndex = iGetFirstIndex();
1511 
1512  T lambda[2], lambdaP[2];
1513 
1514  for (integer i = 1; i <= 2; ++i) {
1515  XCurr.dGetCoef(iFirstIndex + i, lambda[i - 1], 1., &dof);
1516  XCurr.dGetCoef(iFirstIndex + i + 2, lambdaP[i - 1], 1., &dof);
1517  }
1518 
1519  const Vec3 R2o2 = R2 * o2;
1520  const Vec3 l1 = X2 + R2o2 - X1;
1521 
1522  const Vec3 F1 = R1 * Vec3(e.GetCol(2) * lambda[L1] + e.GetCol(3) * lambda[L2]);
1523  const Vec3 M1 = Cross(l1, F1);
1524  const Vec3 FP1 = Cross(omega1, R1 * Vec3(e.GetCol(2) * lambda[L1] + e.GetCol(3) * lambda[L2]))
1525  + R1 * Vec3(e.GetCol(2) * lambdaP[L1] + e.GetCol(3) * lambdaP[L2]);
1526  const Vec3 MP1 = -Cross(F1, XP2 + Cross(omega2, R2o2) - XP1) + Cross(l1, FP1);
1527  const Vec3 F2 = -F1;
1528  const Vec3 M2 = Cross(R2o2, F2);
1529  const Vec3 FP2 = -FP1;
1530  const Vec3 MP2 = Cross(Cross(omega2, R2o2), F2) + Cross(R2o2, FP2);
1531 
1532  const Vec3 a = Transpose(R1) * l1 - o1;
1533  const Vec3 aP = Transpose(R1) * Vec3(Cross(l1, omega1) + XP2 + Cross(omega2, R2o2) - XP1);
1534 
1535  WorkVec.AddItem(iFirstIndexNode1 + 1, F1);
1536  WorkVec.AddItem(iFirstIndexNode1 + 4, M1);
1537  WorkVec.AddItem(iFirstIndexNode1 + 7, FP1);
1538  WorkVec.AddItem(iFirstIndexNode1 + 10, MP1);
1539 
1540  WorkVec.AddItem(iFirstIndexNode2 + 1, F2);
1541  WorkVec.AddItem(iFirstIndexNode2 + 4, M2);
1542  WorkVec.AddItem(iFirstIndexNode2 + 7, FP2);
1543  WorkVec.AddItem(iFirstIndexNode2 + 10, MP2);
1544 
1545  for (int i = 1; i <= 2; ++i) {
1546  WorkVec.AddItem(iFirstIndex + i, Dot(e.GetCol(i + 1), a));
1547  WorkVec.AddItem(iFirstIndex + i + 2, Dot(e.GetCol(i + 1), aP));
1548  }
1549 }
1550 #endif
1551 
1553 {
1554 #ifdef USE_AUTODIFF
1556 
1557  if (!SetUDE("deformable" "joint" "ad", rf))
1558  {
1559  delete rf;
1560  return false;
1561  }
1562 
1563  rf = new UDERead<InlineJointAD>;
1564 
1565  if (!SetUDE("inline" "joint" "ad", rf))
1566  {
1567  delete rf;
1568  return false;
1569  }
1570 
1571  return true;
1572 #else
1573  return false;
1574 #endif
1575 }
1576 
1577 //#ifndef STATIC_MODULES
1578 
1579 extern "C"
1580 {
1581 
1582 int module_init(const char *module_name, void *pdm, void *php)
1583 {
1584  if (!autodiff_test_set())
1585  {
1586  silent_cerr("autodiff_test: "
1587  "module_init(" << module_name << ") "
1588  "failed" << std::endl);
1589 
1590  return -1;
1591  }
1592 
1593  return 0;
1594 }
1595 
1596 }
1597 
1598 //#endif // ! STATIC_MODULE
1599 
1600 
virtual DofOrder::Order GetEqType(unsigned int i) const
Definition: simentity.h:138
flag fReadOutput(MBDynParser &HP, const T &t) const
Definition: dataman.h:1064
Mat3x3 GetRotRel(const ReferenceFrame &rf)
Definition: mbpar.cc:1795
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
const Vec3 Zero3(0., 0., 0.)
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
long int flag
Definition: mbdyn.h:43
MatrixExpression< TransposedMatrix< MatrixDirectExpr< Matrix< T, N_rows, N_cols > > >, N_cols, N_rows > Transpose(const Matrix< T, N_rows, N_cols > &A)
Definition: matvec.h:2206
virtual unsigned int iGetInitialNumDof(void) const =0
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
Definition: matvec3.h:98
virtual void ResizeReset(integer)
Definition: vh.cc:55
const MatCross_Manip MatCross
Definition: matvec3.cc:639
FullSubMatrixHandler & SetFull(void)
Definition: submat.h:1168
double mbdyn_clock_time()
Definition: clock_time.cc:51
virtual unsigned int iGetPrivDataIdx(const char *s) const
Definition: simentity.cc:142
void Put(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:221
const Mat3x3 Eye3(1., 0., 0., 0., 1., 0., 0., 0., 1.)
void PutCoef(integer iRow, integer iCol, const doublereal &dCoef)
Definition: submat.h:672
integer index_type
Definition: gradient.h:104
Definition: matvec3.h:50
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const =0
std::vector< Hint * > Hints
Definition: simentity.h:89
virtual std::ostream & Restart(std::ostream &out) const =0
Vec3 MulTV(const Vec3 &v) const
Definition: matvec3.cc:482
void func(const T &u, const T &v, const T &w, doublereal e, T &f)
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
Vec3 GetPosRel(const ReferenceFrame &rf)
Definition: mbpar.cc:1331
virtual VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)=0
void SetNullMatrix(void)
Definition: submat.h:1159
static const char * dof[]
Definition: drvdisp.cc:195
virtual bool IsKeyWord(const char *sKeyWord)
Definition: parser.cc:910
std::ostream & Loadable(void) const
Definition: output.h:506
FunctionCall
Definition: gradient.h:1018
virtual Mat3x3 GetMat3x3(void)
Definition: mbpar.cc:2438
virtual SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)=0
bool autodiff_test_set(void)
virtual std::ostream & DescribeEq(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: elem.cc:137
DotTraits< VectorExprLhs, VectorExprRhs, N_rows, N_rows >::ExpressionType Dot(const VectorExpression< VectorExprLhs, N_rows > &u, const VectorExpression< VectorExprRhs, N_rows > &v)
Definition: matvec.h:3133
#define ASSERT(expression)
Definition: colamd.c:977
Mat3x3 MulTM(const Mat3x3 &m) const
Definition: matvec3.cc:500
virtual void PutCoef(integer iRow, const doublereal &dCoef)=0
VectorExpression< VectorCrossExpr< VectorLhsExpr, VectorRhsExpr >, 3 > Cross(const VectorExpression< VectorLhsExpr, 3 > &u, const VectorExpression< VectorRhsExpr, 3 > &v)
Definition: matvec.h:3248
virtual unsigned int iGetNumDof(void) const
Definition: elem.cc:118
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
Definition: except.h:79
virtual doublereal dGetPrivData(unsigned int i) const
Definition: simentity.cc:149
virtual unsigned int iGetNumPrivData(void) const
Definition: simentity.cc:136
virtual void GetConnectedNodes(std::vector< const Node * > &connectedNodes) const
Definition: elem.h:243
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
const doublereal * pGetMat(void) const
Definition: matvec3.h:743
virtual void Put(integer iRow, const Vec3 &v)
Definition: vh.cc:93
virtual bool IsArg(void)
Definition: parser.cc:807
Definition: elem.h:75
virtual void Output(OutputHandler &OH) const
Definition: output.cc:870
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
Definition: matvec3.h:49
const doublereal * pGetVec(void) const
Definition: matvec3.h:192
std::ostream & GetLogFile(void) const
Definition: dataman.h:326
bool SetUDE(const std::string &s, UserDefinedElemRead *rude)
Definition: userelem.cc:97
virtual DofOrder::Order GetDofType(unsigned int) const
Definition: elem.cc:150
virtual void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const =0
virtual SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)=0
static const doublereal a
Definition: hfluid_.h:289
Mat3x3 MulMT(const Mat3x3 &m) const
Definition: matvec3.cc:444
virtual void AfterPredict(VectorHandler &X, VectorHandler &XP)
Definition: simentity.cc:91
SparseSubMatrixHandler & SetSparse(void)
Definition: submat.h:1178
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *h=0)
Definition: simentity.cc:63
double doublereal
Definition: colamd.c:52
long int integer
Definition: colamd.c:51
virtual HighParser::ErrOut GetLineData(void) const
Definition: parsinc.cc:697
int module_init(const char *module_name, void *pdm, void *php)
This function registers our user defined element for the math parser.
Node * ReadNode(MBDynParser &HP, Node::Type type) const
Definition: dataman3.cc:2309
virtual std::ostream & DescribeDof(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: elem.cc:124
virtual VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)=0
bool UseText(int out) const
Definition: output.cc:446
virtual void Update(const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: simentity.cc:98