MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
invdataman.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/base/invdataman.cc,v 1.59 2017/09/09 09:20:12 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 /* Inverse Dynamics DataManager */
33 
34 /*
35  * Copyright 2008 Alessandro Fumagalli <alessandro.fumagalli@polimi.it>
36  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
37  */
38 
39 
40 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
41 
42 #include <strings.h>
43 #include <time.h>
44 
45 /* for inertia element */
46 #include <set>
47 
48 #include "dataman.h"
49 #include "friction.h"
50 
51 #include "solver.h"
52 #include "invdyn.h"
53 #include "invsolver.h"
54 #include "constltp.h"
55 #include "dataman_.h"
56 /* add-ons for math parser */
57 #include "dofpgin.h"
58 #include "privpgin.h"
59 #include "dummypgin.h"
60 #include "modelns.h"
61 
62 /* temporary? */
63 #include "beam.h"
64 #include "inertia.h"
65 
66 /* To allow direct loading of modules */
67 #include "modules.h"
68 
69 /* To handle of Elem2Param */
70 #include "j2p.h"
71 
72 /* deformable joint */
73 #include "beam2.h"
74 #include "body.h"
75 #include "joint.h"
76 #include "jointreg.h"
77 #include "Rot.hh"
78 
79 
80 void
81 DataManager::LinkToSolution(VectorHandler& XCurr,
82  VectorHandler& XPrimeCurr,
83  VectorHandler& XPrimePrimeCurr,
84  VectorHandler& LambdaCurr)
85 {
86  pXCurr = &XCurr;
87  pXPrimeCurr = &XPrimeCurr;
88  pXPrimePrimeCurr = &XPrimePrimeCurr;
89  pLambdaCurr = &LambdaCurr;
90 
91  DrvHdl.LinkToSolution(XCurr, XPrimeCurr);
92 }
93 void
94 DataManager::AssConstrJac(MatrixHandler& JacHdl)
95 {
96  DEBUGCOUT("Entering DataManager::AssJac()" << std::endl);
97 
98  ASSERT(pWorkMat != NULL);
99  ASSERT(Elems.begin() != Elems.end());
100 
101  AssConstrJac(JacHdl, ElemIter, *pWorkMat);
102 }
103 
104 void
105 DataManager::AssConstrJac(MatrixHandler& JacHdl,
106  VecIter<Elem *> &Iter,
107  VariableSubMatrixHandler& WorkMat)
108 {
109  DEBUGCOUT("Entering DataManager::AssJac()" << std::endl);
110 
111  JacHdl.Reset();
112 
113  InverseSolver *pIDS = dynamic_cast<InverseSolver *>(pSolver);
114 
115  switch (pIDS->GetProblemType()) {
116  case InverseDynamics::FULLY_ACTUATED_COLLOCATED:
117  for (ElemContainerType::iterator j = ElemData[Elem::JOINT].ElemContainer.begin();
118  j != ElemData[Elem::JOINT].ElemContainer.end(); ++j)
119  {
120  Joint *pJ = Cast<Joint>(j->second);
121  if (pJ->bIsPrescribedMotion()) {
122  ASSERT(pJ->bIsTorque());
123  JacHdl += j->second->AssJac(WorkMat, *pXCurr);
124  }
125  }
126  break;
127 
128  case InverseDynamics::FULLY_ACTUATED_NON_COLLOCATED:
129  case InverseDynamics::UNDERDETERMINED_UNDERACTUATED_COLLOCATED:
130  silent_cerr("DataManager::AssConstrJac(" << pIDS->GetProblemType() << ") not implemented yet" << std::endl);
131  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
132 
133  case InverseDynamics::UNDERDETERMINED_FULLY_ACTUATED: {
134  // NOTE: in this case, the name of the function is misleading,
135  // since it assembles the entire problem's Jacobian matrix
136  // and not only the Jacobian matrix of the constraints
137  InverseDynamicsStepSolver *pIDSS = dynamic_cast<InverseDynamicsStepSolver *>(pSolver->pGetStepIntegrator());
138  if (pIDSS->GetOrder() == InverseDynamics::INVERSE_DYNAMICS) {
139  for (ElemContainerType::iterator j = ElemData[Elem::JOINT].ElemContainer.begin();
140  j != ElemData[Elem::JOINT].ElemContainer.end(); ++j)
141  {
142  bool bActive(true);
143  Joint *pJ = Cast<Joint>(j->second, true);
144  if (pJ == 0) {
145  bActive = false;
146  pJ = Cast<Joint>(j->second, false);
147  }
148 
149  if (pJ->bIsTorque() && bActive) {
150  JacHdl += pJ->AssJac(WorkMat, *pXCurr);
151  WorkMat.AddToT(JacHdl);
152 
153  } else if (pJ->bIsPrescribedMotion() || (pJ->bIsTorque() && !bActive)) {
154  integer iNumDof = pJ->iGetNumDof();
155  integer iFirstIndex = pJ->iGetFirstIndex();
156  for (int iCnt = 1; iCnt <= iNumDof; iCnt++) {
157  JacHdl(iFirstIndex + iCnt, iFirstIndex + iCnt) = 1.;
158  }
159  }
160  }
161 
162  // FIXME: regularization could be needed also in other phases
163  // may need to be related to the state of the regularized joint
164  for (ElemContainerType::iterator j = ElemData[Elem::JOINT_REGULARIZATION].ElemContainer.begin();
165  j != ElemData[Elem::JOINT_REGULARIZATION].ElemContainer.end(); ++j)
166  {
167  JointRegularization *pJ = Cast<JointRegularization>(j->second, true);
168  if (pJ) {
169  JacHdl += pJ->AssJac(WorkMat, *pXCurr);
170  }
171  }
172 
173 #if 0
174  // this _should_ be harmless...
175  for (NodeContainerType::const_iterator n = NodeData[Node::STRUCTURAL].NodeContainer.begin();
176  n != NodeData[Node::STRUCTURAL].NodeContainer.end(); ++n)
177  {
178  ASSERT(n->second->iGetNumDof() == 6);
179  integer iFirstIndex = n->second->iGetFirstIndex();
180 
181  for (integer iCnt = 1; iCnt <= 6; iCnt++) {
182  JacHdl(iFirstIndex + iCnt, iFirstIndex + iCnt) += 1.;
183  }
184  }
185 #endif
186 
187  } else {
188  doublereal dw1, dw2;
189  pIDS->GetWeight(pIDSS->GetOrder(), dw1, dw2);
190  if (dw1 > 0.) {
191  for (NodeContainerType::const_iterator n = NodeData[Node::STRUCTURAL].NodeContainer.begin();
192  n != NodeData[Node::STRUCTURAL].NodeContainer.end(); ++n)
193  {
194  ASSERT(n->second->iGetNumDof() == 6);
195  integer iFirstIndex = n->second->iGetFirstIndex();
196 
197  for (integer iCnt = 1; iCnt <= 6; iCnt++) {
198  JacHdl(iFirstIndex + iCnt, iFirstIndex + iCnt) += dw1;
199  }
200  }
201  }
202 
203  if (dw2 > 0.) {
204 // DO NOT ENABLE, BROKEN
205 //#define TORQUE_OPTIMIZATION
206 #ifdef TORQUE_OPTIMIZATION
207  if (pIDSS->GetOrder() == InverseDynamics::POSITION) {
208  doublereal h = DrvHdl.dGetTimeStep();
209  dw2 /= h*h;
210  }
211 #endif // TORQUE_OPTIMIZATION
212 
213  for (ElemContainerType::const_iterator b = ElemData[Elem::BODY].ElemContainer.begin();
214  b != ElemData[Elem::BODY].ElemContainer.end(); ++b)
215  {
216  if (!b->second->bIsErgonomy()) {
217  continue;
218  }
219 
220  const Body *pBody(Cast<Body>(b->second));
221  doublereal dm(pBody->dGetM()*dw2);
222  Vec3 S(pBody->GetS()*dw2);
223  Mat3x3 J = (pBody->GetJ()*dw2);
224  const StructNode *pNode = pBody->pGetNode();
225  ASSERT(pNode->iGetNumDof() == 6);
226  integer iFirstIndex = pNode->iGetFirstIndex();
227 
228  for (int iRow = 1; iRow <= 3; iRow++) {
229  JacHdl(iFirstIndex + iRow, iFirstIndex + iRow) += dm;
230 
231  for (int iCol = 1; iCol <= 3; iCol++) {
232  JacHdl(iFirstIndex + 3 + iRow, iFirstIndex + 3 + iCol) += J(iRow, iCol);
233  }
234  }
235 
236  JacHdl(iFirstIndex + 3 + 1, iFirstIndex + 2) = -S(3); // 1, 2
237  JacHdl(iFirstIndex + 3 + 1, iFirstIndex + 3) = S(2); // 1, 3
238  JacHdl(iFirstIndex + 3 + 2, iFirstIndex + 3) = -S(1); // 2, 3
239  JacHdl(iFirstIndex + 3 + 2, iFirstIndex + 1) = S(3); // 2, 1
240  JacHdl(iFirstIndex + 3 + 3, iFirstIndex + 1) = -S(2); // 3, 1
241  JacHdl(iFirstIndex + 3 + 3, iFirstIndex + 2) = S(1); // 3, 2
242 
243  JacHdl(iFirstIndex + 2, iFirstIndex + 3 + 1) = -S(3); // 2, 1
244  JacHdl(iFirstIndex + 3, iFirstIndex + 3 + 1) = S(2); // 3, 1
245  JacHdl(iFirstIndex + 3, iFirstIndex + 3 + 2) = -S(1); // 3, 2
246  JacHdl(iFirstIndex + 1, iFirstIndex + 3 + 2) = S(3); // 1, 2
247  JacHdl(iFirstIndex + 1, iFirstIndex + 3 + 3) = -S(2); // 1, 3
248  JacHdl(iFirstIndex + 2, iFirstIndex + 3 + 3) = S(1); // 2, 3
249  }
250  }
251 
252  for (ElemContainerType::iterator j = ElemData[Elem::JOINT].ElemContainer.begin();
253  j != ElemData[Elem::JOINT].ElemContainer.end(); ++j)
254  {
255  bool bActive(true);
256  Joint *pJ = Cast<Joint>(j->second, true);
257  if (pJ == 0) {
258  bActive = false;
259  pJ = Cast<Joint>(j->second, false);
260  }
261 
262  if (pJ->bIsPrescribedMotion() && bActive) {
263  JacHdl += pJ->AssJac(WorkMat, *pXCurr);
264  WorkMat.AddToT(JacHdl);
265 
266  } else if (pJ->bIsErgonomy() && bActive && pIDSS->GetOrder() == InverseDynamics::POSITION) {
267  JacHdl += pJ->AssJac(WorkMat, *pXCurr);
268 
269  } else if (pJ->bIsTorque() || (pJ->bIsPrescribedMotion() && !bActive)) {
270  integer iNumDof = pJ->iGetNumDof();
271  integer iFirstIndex = pJ->iGetFirstIndex();
272  for (int iCnt = 1; iCnt <= iNumDof; iCnt++) {
273  JacHdl(iFirstIndex + iCnt, iFirstIndex + iCnt) = 1.;
274  }
275  }
276  }
277 
278  for (ElemContainerType::iterator j = ElemData[Elem::BEAM].ElemContainer.begin();
279  j != ElemData[Elem::BEAM].ElemContainer.end(); ++j)
280  {
281  bool bActive(true);
282  Beam2 *pB = Cast<Beam2>(j->second, true);
283  if (pB == 0) {
284  bActive = false;
285  pB = Cast<Beam2>(j->second, false);
286  }
287 
288  if (pB && pB->bIsErgonomy() && bActive && pIDSS->GetOrder() == InverseDynamics::POSITION) {
289  JacHdl += pB->AssJac(WorkMat, *pXCurr);
290  }
291  }
292  }
293  } break;
294 
295  default:
296  ASSERT(0);
297  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
298  }
299 }
300 
301 /* Constraint residual assembly: */
302 void
303 DataManager::AssConstrRes(VectorHandler& ResHdl, InverseDynamics::Order iOrder)
304 {
305  DEBUGCOUT("Entering AssRes()" << std::endl);
306 
307  AssConstrRes(ResHdl, ElemIter, *pWorkVec, iOrder);
308 }
309 
310 void
311 DataManager::AssConstrRes(VectorHandler& ResHdl,
312  VecIter<Elem *> &Iter,
313  SubVectorHandler& WorkVec,
314  InverseDynamics::Order iOrder)
315 {
316  DEBUGCOUT("Entering AssRes()" << std::endl);
317 
318  // TODO
319 
320  bool ChangedEqStructure(false);
321  InverseSolver *pIDS = dynamic_cast<InverseSolver *>(pSolver);
322 
323  switch (pIDS->GetProblemType()) {
324  case InverseDynamics::FULLY_ACTUATED_COLLOCATED:
325  for (ElemContainerType::iterator j = ElemData[Elem::JOINT].ElemContainer.begin();
326  j != ElemData[Elem::JOINT].ElemContainer.end(); ++j)
327  {
328  Joint *pJ = Cast<Joint>(j->second);
329  if (pJ->bIsPrescribedMotion()) {
330  ASSERT(pJ->bIsTorque());
331  try {
332  ResHdl += pJ->AssRes(WorkVec, *pXCurr,
333  *pXPrimeCurr, *pXPrimePrimeCurr, iOrder);
334  }
335  catch (Elem::ChangedEquationStructure) {
336  ResHdl += WorkVec;
337  ChangedEqStructure = true;
338  }
339  }
340  }
341  break;
342 
343  case InverseDynamics::FULLY_ACTUATED_NON_COLLOCATED:
344  case InverseDynamics::UNDERDETERMINED_UNDERACTUATED_COLLOCATED:
345  silent_cerr("DataManager::AssConstrRes(" << pIDS->GetProblemType() << ") not implemented yet" << std::endl);
346  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
347 
348  case InverseDynamics::UNDERDETERMINED_FULLY_ACTUATED: {
349  // NOTE: in this case, the name of the function is misleading,
350  // since it assembles the entire problem's residual
351  // and not only the residual of the constraints
352  doublereal dw1, dw2;
353  pIDS->GetWeight(iOrder, dw1, dw2);
354  switch (iOrder) {
355  case InverseDynamics::POSITION:
356  if (dw1 > 0.) {
357  for (NodeContainerType::const_iterator n = NodeData[Node::STRUCTURAL].NodeContainer.begin();
358  n != NodeData[Node::STRUCTURAL].NodeContainer.end(); ++n)
359  {
360  const StructNode *pNode = dynamic_cast<const StructNode *>(n->second);
361 
362  ASSERT(pNode->iGetNumDof() == 6);
363  integer iFirstIndex = pNode->iGetFirstIndex();
364 
365  Vec3 DX(pNode->GetXPrev() - pNode->GetXCurr());
366  for (integer iCnt = 1; iCnt <= 3; iCnt++) {
367  ResHdl(iFirstIndex + iCnt) += dw1*DX(iCnt);
368  }
369 
370  // VecRot(Rp*Rc^T) = -VecRot(Rc*Rp^T)
371  Vec3 DTheta(RotManip::VecRot(pNode->GetRPrev().MulMT(pNode->GetRCurr())));
372  for (integer iCnt = 1; iCnt <= 3; iCnt++) {
373  ResHdl(iFirstIndex + 3 + iCnt) += dw1*DTheta(iCnt);
374  }
375  }
376  }
377 
378  if (dw2 > 0.) {
379 #ifdef TORQUE_OPTIMIZATION
380  doublereal h = DrvHdl.dGetTimeStep();
381  for (ElemContainerType::const_iterator b = ElemData[Elem::BODY].ElemContainer.begin();
382  b != ElemData[Elem::BODY].ElemContainer.end(); ++b)
383  {
384  if (!b->second->bIsRightHandSide()) {
385  continue;
386  }
387 
388  const Body *pBody(Cast<Body>(b->second));
389  doublereal dm(pBody->dGetM()*dw2);
390  Vec3 S(pBody->GetS()*dw2);
391  Mat3x3 J = (pBody->GetJ()*dw2);
392  const StructNode *pNode = pBody->pGetNode();
393  ASSERT(pNode->iGetNumDof() == 6);
394  integer iFirstIndex = pNode->iGetFirstIndex();
395 
396  Vec3 DXP((pNode->GetXCurr() - pNode->GetXPrev())/h);
397  Vec3 DXPP((DXP - pNode->GetVPrev())/h);
398  // VecRot(Rp*Rc^T) = -VecRot(Rc*Rp^T)
399  Vec3 DW(RotManip::VecRot(pNode->GetRCurr().MulMT(pNode->GetRPrev()))/h);
400  Vec3 DWP((DW - pNode->GetWPrev())/h);
401 
402  Vec3 XRes(DXPP*dm + DWP.Cross(S) + DW.Cross(DW.Cross(S)));
403  for (integer iCnt = 1; iCnt <= 3; iCnt++) {
404  ResHdl(iFirstIndex + iCnt) -= XRes(iCnt);
405  }
406 
407  Vec3 ThetaRes(S.Cross(DXPP) + J*DWP + DW.Cross(J*DW));
408  for (integer iCnt = 1; iCnt <= 3; iCnt++) {
409  ResHdl(iFirstIndex + 3 + iCnt) -= ThetaRes(iCnt);
410  }
411  }
412 #else // !TORQUE_OPTIMIZATION
413  for (ElemContainerType::const_iterator b = ElemData[Elem::BODY].ElemContainer.begin();
414  b != ElemData[Elem::BODY].ElemContainer.end(); ++b)
415  {
416  if (!b->second->bIsErgonomy()) {
417  continue;
418  }
419 
420  const Body *pBody(Cast<Body>(b->second));
421  doublereal dm(pBody->dGetM()*dw2);
422  Vec3 S(pBody->GetS()*dw2);
423  Mat3x3 J = (pBody->GetJ()*dw2);
424  const StructNode *pNode = pBody->pGetNode();
425  ASSERT(pNode->iGetNumDof() == 6);
426  integer iFirstIndex = pNode->iGetFirstIndex();
427 
428  Vec3 DX(pNode->GetXPrev() - pNode->GetXCurr());
429  // VecRot(Rp*Rc^T) = -VecRot(Rc*Rp^T)
430  Vec3 DTheta(RotManip::VecRot(pNode->GetRPrev().MulMT(pNode->GetRCurr())));
431 
432  Vec3 XRes(DX*dm - S.Cross(DTheta));
433  for (integer iCnt = 1; iCnt <= 3; iCnt++) {
434  ResHdl(iFirstIndex + iCnt) += XRes(iCnt);
435  }
436 
437  Vec3 ThetaRes(S.Cross(DX) + J*DTheta);
438  for (integer iCnt = 1; iCnt <= 3; iCnt++) {
439  ResHdl(iFirstIndex + 3 + iCnt) += ThetaRes(iCnt);
440  }
441  }
442 #endif // !TORQUE_OPTIMIZATION
443  }
444  break;
445 
446  case InverseDynamics::VELOCITY:
447  if (dw1 > 0.) {
448  doublereal h = DrvHdl.dGetTimeStep();
449 
450  // xp_k = xp_km1/3 + 2/3*(x_k - x_km1)/h
451  // xp_k = 2*(x_k - x_km1)/h - xp_km1
452  for (NodeContainerType::const_iterator n = NodeData[Node::STRUCTURAL].NodeContainer.begin();
453  n != NodeData[Node::STRUCTURAL].NodeContainer.end(); ++n)
454  {
455  const StructNode *pNode = dynamic_cast<const StructNode *>(n->second);
456 
457  ASSERT(pNode->iGetNumDof() == 6);
458  integer iFirstIndex = pNode->iGetFirstIndex();
459 
460  const Vec3& VPrev(pNode->GetVPrev());
461  const Vec3& XPrev(pNode->GetXPrev());
462  const Vec3& XCurr(pNode->GetXCurr());
463 
464 // #define USE_2XmV 1 // 2nd order, a-stable, oscillations
465 // #define USE_2XpVd3 1 // 1st order, a/l-stable, more accurate
466 #define USE_X 1 // 1st order, l-stable (implicit Euler), less accurate (alpha == 1.)
467 // #define USE_alphaX_betaV (1.0) // alpha = 1. + |rho|; beta = (1 - alpha) for 1st order accuracy
468 
469 #if USE_2XmV
470  Vec3 VRef((XCurr - XPrev)*(2./h) - VPrev);
471 #elif USE_2XpVd3
472  Vec3 VRef((XCurr - XPrev)*(2./3./h) + VPrev/3.);
473 #elif defined(USE_alphaX_betaV)
474  Vec3 VRef((XCurr - XPrev)*(USE_alphaX_betaV/h) + VPrev*(1 - USE_alphaX_betaV));
475 #elif USE_X
476  Vec3 VRef((XCurr - XPrev)/h);
477 #endif
478  for (integer iCnt = 1; iCnt <= 3; iCnt++) {
479  ResHdl(iFirstIndex + iCnt) += dw1*VRef(iCnt);
480  }
481 
482  const Vec3& WPrev(pNode->GetWPrev());
483  const Mat3x3& RPrev(pNode->GetRPrev());
484  const Mat3x3& RCurr(pNode->GetRCurr());
485 
486 #if USE_2XmV
487  Vec3 WRef(RotManip::VecRot(RCurr.MulMT(RPrev))*(2./h) - WPrev);
488 #elif USE_2XpVd3
489  Vec3 WRef(RotManip::VecRot(RCurr.MulMT(RPrev))*(2./3./h) + WPrev/3.);
490 #elif defined(USE_alphaX_betaV)
491  Vec3 WRef(RotManip::VecRot(RCurr.MulMT(RPrev))*(USE_alphaX_betaV/h) + WPrev*(1 - USE_alphaX_betaV));
492 #elif USE_X
493  Vec3 WRef(RotManip::VecRot(RCurr.MulMT(RPrev))/h);
494 #endif
495  for (integer iCnt = 1; iCnt <= 3; iCnt++) {
496  ResHdl(iFirstIndex + 3 + iCnt) += dw1*WRef(iCnt);
497  }
498  }
499  }
500 
501  if (dw2 > 0.) {
502  for (ElemContainerType::const_iterator b = ElemData[Elem::BODY].ElemContainer.begin();
503  b != ElemData[Elem::BODY].ElemContainer.end(); ++b)
504  {
505  if (!b->second->bIsErgonomy()) {
506  continue;
507  }
508 
509  const Body *pBody(Cast<Body>(b->second));
510  doublereal dm(pBody->dGetM()*dw2);
511  Vec3 S(pBody->GetS()*dw2);
512  Mat3x3 J = (pBody->GetJ()*dw2);
513  const StructNode *pNode = pBody->pGetNode();
514  ASSERT(pNode->iGetNumDof() == 6);
515  integer iFirstIndex = pNode->iGetFirstIndex();
516 
517  Vec3 BPrev(pNode->GetVPrev()*dm - S.Cross(pNode->GetWPrev()));
518  for (integer iCnt = 1; iCnt <= 3; iCnt++) {
519  ResHdl(iFirstIndex + iCnt) += BPrev(iCnt);
520  }
521 
522  Vec3 GPrev(S.Cross(pNode->GetVPrev()) + J*pNode->GetWPrev());
523  for (integer iCnt = 1; iCnt <= 3; iCnt++) {
524  ResHdl(iFirstIndex + 3 + iCnt) += GPrev(iCnt);
525  }
526  }
527  }
528  break;
529 
530  case InverseDynamics::ACCELERATION:
531  if (dw1 > 0.) {
532  doublereal h = DrvHdl.dGetTimeStep();
533 
534  // xpp_k = xpp_km1/3 + 2/3*(xp_k - xp_km1)/h
535  // xpp_k = 2*(xp_k - xp_km1)/h - xpp_km1
536  // xpp_k = xpp_km1 + 6*(xp_k + xp_km1)/h - 12*(x_k - x_km1)/h^2
537  for (NodeContainerType::const_iterator n = NodeData[Node::STRUCTURAL].NodeContainer.begin();
538  n != NodeData[Node::STRUCTURAL].NodeContainer.end(); ++n)
539  {
540  const StructNode *pNode = dynamic_cast<const StructNode *>(n->second);
541 
542  ASSERT(pNode->iGetNumDof() == 6);
543  integer iFirstIndex = pNode->iGetFirstIndex();
544 
545  const Vec3& XPPPrev(pNode->GetXPPPrev());
546  const Vec3& VPrev(pNode->GetVPrev());
547  const Vec3& VCurr(pNode->GetVCurr());
548 
549 #if USE_2XmV
550  Vec3 XPPRef((VCurr - VPrev)*(2./h) - XPPPrev);
551 #elif USE_2XpVd3
552  Vec3 XPPRef((VCurr - VPrev)*(2./3./h) + XPPPrev/3.);
553 #elif defined(USE_alphaX_betaV)
554  Vec3 XPPRef((VCurr - VPrev)*(USE_alphaX_betaV/h) + XPPPrev*(1 - USE_alphaX_betaV));
555 #elif USE_X
556  Vec3 XPPRef((VCurr - VPrev)/h);
557 #endif
558 #if 0
559  const Vec3& XPrev(pNode->GetXPrev());
560  const Vec3& XCurr(pNode->GetXCurr());
561 
562  Vec3 XPPRef(XPPPrev + (VCurr + VPrev)*(6./h) - (XCurr - XPrev)*(12./h/h));
563 #endif
564  for (integer iCnt = 1; iCnt <= 3; iCnt++) {
565  ResHdl(iFirstIndex + iCnt) += dw1*XPPRef(iCnt);
566  }
567 
568  const Vec3& WPPrev(pNode->GetWPPrev());
569  const Vec3& WPrev(pNode->GetWPrev());
570  const Vec3& WCurr(pNode->GetWCurr());
571 
572 #if USE_2XmV
573  Vec3 WPRef((WCurr - WPrev)*(2./h) - WPPrev);
574 #elif USE_2XpVd3
575  Vec3 WPRef((WCurr - WPrev)*(2./3./h) + WPPrev/3.);
576 #elif defined(USE_alphaX_betaV)
577  Vec3 WPRef((WCurr - WPrev)*(USE_alphaX_betaV/h) + WPPrev*(1 - USE_alphaX_betaV));
578 #elif USE_X
579  Vec3 WPRef((WCurr - WPrev)/h);
580 #endif
581 #if 0
582  const Mat3x3& RPrev(pNode->GetRPrev());
583  const Mat3x3& RCurr(pNode->GetRCurr());
584 
585  Vec3 WPRef(WPPrev + (WCurr + WPrev)*(6./h) - RotManip::VecRot(RCurr.MulMT(RPrev))*(12./h/h));
586 #endif
587  for (integer iCnt = 1; iCnt <= 3; iCnt++) {
588  ResHdl(iFirstIndex + 3 + iCnt) += dw1*WPRef(iCnt);
589  }
590  }
591  }
592 
593  if (dw2 > 0.) {
594  for (ElemContainerType::const_iterator b = ElemData[Elem::BODY].ElemContainer.begin();
595  b != ElemData[Elem::BODY].ElemContainer.end(); ++b)
596  {
597  if (!b->second->bIsErgonomy()) {
598  continue;
599  }
600 
601  const Body *pBody(Cast<Body>(b->second));
602  doublereal dm(pBody->dGetM()*dw2);
603  Vec3 S(pBody->GetS()*dw2);
604  Mat3x3 J = (pBody->GetJ()*dw2);
605  const StructNode *pNode = pBody->pGetNode();
606  ASSERT(pNode->iGetNumDof() == 6);
607  integer iFirstIndex = pNode->iGetFirstIndex();
608 
609  Vec3 BPPrev(pNode->GetXPPPrev()*dm - S.Cross(pNode->GetWPPrev()));
610  for (integer iCnt = 1; iCnt <= 3; iCnt++) {
611  ResHdl(iFirstIndex + iCnt) += BPPrev(iCnt);
612  }
613 
614  Vec3 GPPrev(S.Cross(pNode->GetXPPPrev()) + J*pNode->GetWPPrev());
615  for (integer iCnt = 1; iCnt <= 3; iCnt++) {
616  ResHdl(iFirstIndex + 3 + iCnt) += GPPrev(iCnt);
617  }
618  }
619  }
620  break;
621 
622  default:
623  ASSERT(0);
624  }
625 
626  for (ElemContainerType::iterator j = ElemData[Elem::JOINT].ElemContainer.begin();
627  j != ElemData[Elem::JOINT].ElemContainer.end(); ++j)
628  {
629  bool bActive(true);
630  Joint *pJ = Cast<Joint>(j->second, true);
631  if (pJ == 0) {
632  bActive = false;
633  pJ = Cast<Joint>(j->second, false);
634  }
635 
636  if ((pJ->bIsPrescribedMotion()
637  || (iOrder == InverseDynamics::POSITION && pJ->bIsErgonomy())) && bActive)
638  {
639  try {
640  ResHdl += j->second->AssRes(WorkVec, *pXCurr,
641  *pXPrimeCurr, *pXPrimePrimeCurr, iOrder);
642  }
643  catch (Elem::ChangedEquationStructure) {
644  ResHdl += WorkVec;
645  ChangedEqStructure = true;
646  }
647 
648  } else if (pJ->bIsTorque() || (pJ->bIsPrescribedMotion() && !bActive)) {
649  integer iNumDof = pJ->iGetNumDof();
650  integer iFirstIndex = pJ->iGetFirstIndex();
651  if (iOrder == InverseDynamics::POSITION) {
652  for (int iCnt = 1; iCnt <= iNumDof; iCnt++) {
653  ResHdl(iFirstIndex + iCnt) = -(*pXCurr)(iFirstIndex + iCnt);
654  }
655 
656  } else {
657  for (int iCnt = 1; iCnt <= iNumDof; iCnt++) {
658  ResHdl(iFirstIndex + iCnt) = 0.;
659  }
660  }
661  }
662  }
663 
664  for (ElemContainerType::iterator j = ElemData[Elem::BEAM].ElemContainer.begin();
665  j != ElemData[Elem::BEAM].ElemContainer.end(); ++j)
666  {
667  bool bActive(true);
668  Beam2 *pB = Cast<Beam2>(j->second, true);
669  if (pB == 0) {
670  bActive = false;
671  pB = Cast<Beam2>(j->second, false);
672  }
673 
674  if (pB && (iOrder == InverseDynamics::POSITION && pB->bIsErgonomy()) && bActive)
675  {
676  try {
677  ResHdl += j->second->AssRes(WorkVec, *pXCurr,
678  *pXPrimeCurr, *pXPrimePrimeCurr, iOrder);
679  }
680  catch (Elem::ChangedEquationStructure) {
681  ResHdl += WorkVec;
682  ChangedEqStructure = true;
683  }
684  }
685  }
686 
687  } break;
688 
689  default:
690  ASSERT(0);
691  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
692  }
693 
694  if (ChangedEqStructure) {
695  throw ChangedEquationStructure(MBDYN_EXCEPT_ARGS);
696  }
697 }
698 
699 /* Equilibrium residual assembly, no constraints */
700 void
701 DataManager::AssRes(VectorHandler& ResHdl)
702 {
703  DEBUGCOUT("Entering AssRes()" << std::endl);
704  AssRes(ResHdl, ElemIter, *pWorkVec);
705 
706  for (ElemContainerType::iterator j = ElemData[Elem::INERTIA].ElemContainer.begin();
707  j != ElemData[Elem::INERTIA].ElemContainer.end(); ++j)
708  {
709  bool bActive(true);
710  Inertia *pI = Cast<Inertia>(j->second, true);
711  if (pI == 0) {
712  bActive = false;
713  pI = Cast<Inertia>(j->second, false);
714  }
715  if (pI && bActive)
716  {
717  ResHdl += j->second->AssRes(*pWorkVec, *pXCurr,
718  *pXPrimeCurr, *pXPrimePrimeCurr,
719  InverseDynamics::INVERSE_DYNAMICS);
720  }
721  }
722 }
723 
724 void
725 DataManager::AssRes(VectorHandler& ResHdl,
726  VecIter<Elem *> &Iter,
727  SubVectorHandler& WorkVec)
728 {
729  DEBUGCOUT("Entering AssRes()" << std::endl);
730 
731  // TODO
732  // FIXME: could be as the rest?
733 
734  const Elem::Type ElemType[] = {
735  Elem::BODY,
736  Elem::BEAM,
737  Elem::FORCE,
738 
739  Elem::LASTELEMTYPE
740  };
741 
742  bool ChangedEqStructure(false);
743 
744  for (int et = 0; ElemType[et] != Elem::LASTELEMTYPE; et++) {
745  for (ElemContainerType::iterator j = ElemData[ElemType[et]].ElemContainer.begin();
746  j != ElemData[ElemType[et]].ElemContainer.end(); ++j)
747  {
748  if (!j->second->bIsRightHandSide()) {
749  continue;
750  }
751 
752  try {
753  ResHdl += j->second->AssRes(WorkVec, *pXCurr,
754  *pXPrimeCurr, *pXPrimePrimeCurr,
755  InverseDynamics::INVERSE_DYNAMICS);
756  }
757  catch (Elem::ChangedEquationStructure) {
758  ResHdl += WorkVec;
759  ChangedEqStructure = true;
760  }
761  }
762  }
763 
764  for (ElemContainerType::iterator j = ElemData[Elem::JOINT].ElemContainer.begin();
765  j != ElemData[Elem::JOINT].ElemContainer.end(); ++j)
766  {
767  bool bActive(true);
768  Joint *pJ = Cast<Joint>(j->second, true);
769  if (pJ == 0) {
770  bActive = false;
771  pJ = Cast<Joint>(j->second, false);
772  }
773 
774  if (bActive && pJ->bIsRightHandSide()) {
775  try {
776  ResHdl += pJ->AssRes(WorkVec, *pXCurr,
777  *pXPrimeCurr, *pXPrimePrimeCurr,
778  InverseDynamics::INVERSE_DYNAMICS);
779  }
780  catch (Elem::ChangedEquationStructure) {
781  ResHdl += WorkVec;
782  ChangedEqStructure = true;
783  }
784  }
785  }
786 
787  if (ChangedEqStructure) {
788  throw ChangedEquationStructure(MBDYN_EXCEPT_ARGS);
789  }
790 }
791 
792 void
793 DataManager::Update(InverseDynamics::Order iOrder) const
794 {
795  const VectorHandler* pV = 0;
796 
797  switch (iOrder) {
798  case InverseDynamics::INVERSE_DYNAMICS:
799  // Update constraints reactions (for output only...)
800  for (ElemContainerType::const_iterator j = ElemData[Elem::JOINT].ElemContainer.begin();
801  j != ElemData[Elem::JOINT].ElemContainer.end(); ++j)
802  {
803  j->second->Update(*pLambdaCurr, iOrder);
804  }
805  return;
806 
807  // Nodes:
808  case InverseDynamics::POSITION:
809  // Update nodes positions
810  pV = pXCurr;
811  break;
812 
813  case InverseDynamics::VELOCITY:
814  // Update nodes velocities
815  pV = pXPrimeCurr;
816  break;
817 
818  case InverseDynamics::ACCELERATION:
819  // Update nodes accelerations
820  pV = pXPrimePrimeCurr;
821  break;
822 
823  default:
824  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
825  }
826 
827  ASSERT(pV != 0);
828  for (NodeVecType::const_iterator i = Nodes.begin(); i != Nodes.end(); ++i) {
829  (*i)->Update(*pV, iOrder);
830  }
831 }
832 
833 void
834 DataManager::IDAfterConvergence(void) const
835 {
836  DEBUGCOUTFNAME("DataManager::IDAfterConvergence");
837 
838  // Nodes:
839  for (NodeVecType::const_iterator i = Nodes.begin(); i != Nodes.end(); ++i) {
840  (*i)->AfterConvergence(*pXCurr, *pXPrimeCurr, *pXPrimePrimeCurr);
841  }
842 
843  Elem* pEl = NULL;
844  if (ElemIter.bGetFirst(pEl)) {
845  do {
846  pEl->AfterConvergence(*pXCurr, *pXPrimeCurr, *pXPrimePrimeCurr);
847  } while (ElemIter.bGetNext(pEl));
848  }
849 }
850 
851 void
852 DataManager::IDDofOwnerSet(void)
853 {
854  DEBUGCOUTFNAME("DataManager::IDDofOwnerSet");
855 
856  /* Setta i DofOwner dei nodi */
857  for (NodeVecType::const_iterator i = Nodes.begin(); i != Nodes.end(); ++i) {
858  DofOwner* pDO = const_cast<DofOwner *>((*i)->pGetDofOwner());
859  pDO->iNumDofs = (*i)->iGetNumDof();
860  iIDNodeTotNumDofs += pDO->iNumDofs;
861  }
862 
863  for (ElemContainerType::iterator j = ElemData[Elem::JOINT].ElemContainer.begin();
864  j != ElemData[Elem::JOINT].ElemContainer.end(); ++j)
865  {
866  ElemWithDofs* pEWD = Cast<ElemWithDofs>(j->second);
867  iIDJointTotNumDofs += pEWD->iGetNumDof();
868  }
869 
870  /* Setta i DofOwner degli elementi (chi li possiede) */
871  for (int iCnt = 0; iCnt < Elem::LASTELEMTYPE; iCnt++) {
872  DofOwner::Type DT = ElemData[iCnt].DofOwnerType;
873  if (DT != DofOwner::UNKNOWN) {
874  DEBUGLCOUT(MYDEBUG_INIT, "Elem type " << iCnt
875  << " (" << psElemNames[iCnt] << ")"
876  << std::endl);
877 
878  for (ElemContainerType::const_iterator p = ElemData[iCnt].ElemContainer.begin();
879  p != ElemData[iCnt].ElemContainer.end(); ++p)
880  {
881  ElemWithDofs* pEWD = Cast<ElemWithDofs>(p->second);
882 
883  DEBUGLCOUT(MYDEBUG_INIT, " " << psElemNames[pEWD->GetElemType()]
884  << "(" << pEWD->GetLabel() << ")" << std::endl);
885 
886  DofOwner* pDO = const_cast<DofOwner *>(pEWD->pGetDofOwner());
887  pDO->iNumDofs = pEWD->iGetNumDof();
888  DEBUGLCOUT(MYDEBUG_INIT, " num dofs: " << pDO->iNumDofs << std::endl);
889  }
890  }
891  }
892 }
893 
894 void
895 DataManager::IDDofInit(void)
896 {
897  if (iTotDofOwners == 0) {
898  silent_cerr("DataManager::IDDofInit: no dof owners are defined" << std::endl);
899 
900  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
901  }
902 
903  /* Di ogni DofOwner setta il primo indice
904  * e calcola il numero totale di Dof:
905  * poichè per il problema inverso non si
906  * possono aggiungere incognite diverse
907  * da posizione (velocità e accelerazione)
908  * dei nodi e reazioni vincolari, viene
909  * controllato che gli elementi che aggiungono
910  * dof siano solo nodi e vincoli.
911  *
912  * Per problemi mal posti (DoF nodi != Dof vincoli):
913  * iTotDofs = (DoF nodi) + (Dof vincoli), altrimenti
914  *
915  * Per problemi ben posti:
916  * iTotDofs = DoF nodi (= DoF vincoli)
917  */
918 
919  integer iRealTotDofs = 0;
920 
921  /* Mette gli indici ai DofOwner dei nodi strutturali: */
922  /* contatore dei Dof dei nodi */
923  integer iNodeIndex = 0;
924 
925  NodeContainerType::const_iterator i = NodeData[Node::STRUCTURAL].NodeContainer.begin();
926  for (int iDOm1 = 0; iDOm1 < DofData[DofOwner::STRUCTURALNODE].iNum;
927  ++iDOm1, ++i)
928  {
929  DofOwner *pDO = const_cast<DofOwner *>(i->second->pGetDofOwner());
930  unsigned iNumDofs = pDO->iNumDofs = i->second->iGetNumDof();
931  if (iNumDofs > 0) {
932  pDO->iFirstIndex = iNodeIndex;
933  iNodeIndex += iNumDofs;
934  iRealTotDofs += iNumDofs;
935 
936  } else {
937  // note: this could only be possible for dummy nodes?
938  pDO->iFirstIndex = -1;
939  DEBUGCERR("warning, Structural(" << i->second->GetLabel() << ") "
940  "(DofOwner #" << (iDOm1 + 1) << ") has 0 dofs" << std::endl);
941  }
942  }
943 
944  ASSERT(iNodeIndex == iIDNodeTotNumDofs);
945 
946  /* Gli indici dei nodi sono ok */
947 
948  if (bOutputAccels) {
949  for (NodeContainerType::iterator n = NodeData[Node::STRUCTURAL].NodeContainer.begin();
950  n != NodeData[Node::STRUCTURAL].NodeContainer.end(); ++n)
951  {
952  dynamic_cast<StructNode *>(n->second)->OutputAccelerations(true);
953  }
954  }
955 
956  /* Se il problema è ben posto, gli indici delle equazioni di vincolo
957  * hanno numerazione indipendente dai nodi. Altrimenti la numerazione
958  * è a partire dagli indici dei nodi (per fare spazio alla matrice
959  * peso nello jacobiano) */
960 
961  /* contatore dei Dof dei joint */
962  integer iJointIndex;
963  integer iPrescribedMotionJointIndex;
964  integer iTorqueJointIndex;
965  switch (dynamic_cast<InverseSolver *>(pSolver)->GetProblemType()) {
966  case InverseDynamics::FULLY_ACTUATED_COLLOCATED:
967  if (iIDNodeTotNumDofs != iIDJointTotNumDofs) {
968  silent_cerr("DataManager::IDDofInit: nodeDoFs=" << iIDNodeTotNumDofs
969  << " jointDoFs=" << iIDJointTotNumDofs << std::endl);
970  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
971  }
972 
973  case InverseDynamics::FULLY_ACTUATED_NON_COLLOCATED:
974  iJointIndex = 0;
975  iPrescribedMotionJointIndex = 0;
976  iTorqueJointIndex = 0;
977  break;
978 
979  case InverseDynamics::UNDERDETERMINED_UNDERACTUATED_COLLOCATED:
980  case InverseDynamics::UNDERDETERMINED_FULLY_ACTUATED:
981  iJointIndex = iNodeIndex;
982  iPrescribedMotionJointIndex = iNodeIndex;
983  iTorqueJointIndex = iNodeIndex;
984  break;
985 
986  default:
987  ASSERT(0);
988  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
989  }
990 
991  for (ElemContainerType::iterator j = ElemData[Elem::JOINT].ElemContainer.begin();
992  j != ElemData[Elem::JOINT].ElemContainer.end(); ++j)
993  {
994  Joint *pJ = Cast<Joint>(j->second);
995  ASSERT(pJ != 0);
996 
997  DofOwner *pDO = const_cast<DofOwner *>(pJ->pGetDofOwner());
998  ASSERT(pDO != 0);
999 
1000  unsigned iNumDofs = pDO->iNumDofs;
1001  if (iNumDofs > 0) {
1002  pDO->iFirstIndex = iJointIndex;
1003  iRealTotDofs += iNumDofs;
1004  iJointIndex += iNumDofs;
1005  if (pJ->bIsPrescribedMotion()) {
1006  iPrescribedMotionJointIndex += iNumDofs;
1007  }
1008  if (pJ->bIsTorque()) {
1009  iTorqueJointIndex += iNumDofs;
1010  }
1011 
1012  } else {
1013  pDO->iFirstIndex = -1;
1014  DEBUGCERR("warning, Joint(" << j->second->GetLabel() << ") "
1015  "has 0 dofs" << std::endl);
1016  }
1017 
1018  const char *sTorque = pJ->bIsTorque() ? "T" : "_";
1019  const char *sPrescribedMotion = pJ->bIsPrescribedMotion() ? "P" : "_";
1020  const char *sErgonomy = pJ->bIsErgonomy() ? "E" : "_";
1021  const char *sRightHandSide = pJ->bIsRightHandSide() ? "R" : "_";
1022 
1023  silent_cout("Joint(" << pJ->GetLabel() << "): " << sTorque << sPrescribedMotion << sErgonomy << sRightHandSide << std::endl);
1024  }
1025 
1026  for (ElemContainerType::iterator j = ElemData[Elem::BEAM].ElemContainer.begin();
1027  j != ElemData[Elem::BEAM].ElemContainer.end(); ++j)
1028  {
1029  Beam2 *pB = Cast<Beam2>(j->second);
1030  ASSERT(pB != 0);
1031 
1032  const char *sTorque = "_";
1033  const char *sPrescribedMotion = "_";
1034  const char *sErgonomy = pB->bIsErgonomy() ? "E" : "_";
1035  const char *sRightHandSide = pB->bIsRightHandSide() ? "R" : "_";
1036 
1037  silent_cout("Beam2(" << pB->GetLabel() << "): " << sTorque << sPrescribedMotion << sErgonomy << sRightHandSide << std::endl);
1038  }
1039 
1040  switch (dynamic_cast<InverseSolver *>(pSolver)->GetProblemType()) {
1041  case InverseDynamics::FULLY_ACTUATED_COLLOCATED:
1042  ASSERT(iIDNodeTotNumDofs == iIDJointTotNumDofs);
1043  // fall thru
1044 
1045  case InverseDynamics::FULLY_ACTUATED_NON_COLLOCATED:
1046  iTotDofs = iNodeIndex;
1047  break;
1048 
1049  case InverseDynamics::UNDERDETERMINED_UNDERACTUATED_COLLOCATED:
1050  case InverseDynamics::UNDERDETERMINED_FULLY_ACTUATED:
1051  iTotDofs = iTorqueJointIndex;
1052  break;
1053 
1054  default:
1055  break;
1056  }
1057 
1058  iTotDofs = iJointIndex;
1059 
1060  DEBUGLCOUT(MYDEBUG_INIT, "iTotDofs = " << iTotDofs << std::endl);
1061 
1062 
1063  /* Crea la struttura dinamica dei Dof */
1064  if (iTotDofs == 0) {
1065  silent_cerr("DataManager::IDDofInit: no dofs defined" << std::endl);
1066  throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
1067  }
1068 
1069  Dofs.resize(iRealTotDofs); /* Inizializza l'iteratore sui Dof */
1070 
1071  /* Inizializza la struttura dinamica dei Dof */
1072  integer iIndex = DofOwners[0].iFirstIndex;
1073  for (integer idx = 0; idx < iRealTotDofs; idx++) {
1074  Dofs[idx].iIndex = iIndex++;
1075  Dofs[idx].Order = DofOrder::DIFFERENTIAL;
1076  }
1077 }
1078 
1079 int
1080 DataManager::iIDGetNodeTotNumDofs(void) const
1081 {
1082  return iIDNodeTotNumDofs;
1083 }
1084 
1085 int
1086 DataManager::iIDGetJointTotNumDofs(void) const
1087 {
1088  return iIDJointTotNumDofs;
1089 }
1090 
1091 int
1092 DataManager::iIDGetTotNumDofs(void) const
1093 {
1094  switch (dynamic_cast<InverseSolver *>(pSolver)->GetProblemType()) {
1095  case InverseDynamics::FULLY_ACTUATED_COLLOCATED:
1096  case InverseDynamics::FULLY_ACTUATED_NON_COLLOCATED:
1097  ASSERT(iIDNodeTotNumDofs == iIDJointTotNumDofs);
1098  return iIDNodeTotNumDofs;
1099 
1100  case InverseDynamics::UNDERDETERMINED_UNDERACTUATED_COLLOCATED:
1101  case InverseDynamics::UNDERDETERMINED_FULLY_ACTUATED:
1102  return iIDNodeTotNumDofs + iIDJointTotNumDofs;
1103 
1104  default:
1105  ASSERT(0);
1106  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1107  }
1108 }
1109 
1110 void
1111 DataManager::IDSetTest(NonlinearSolverTestRange *pResTest, NonlinearSolverTestRange *pSolTest, bool bFullResTest)
1112 {
1113  integer iFirstIndex = std::numeric_limits<integer>::max();
1114  integer iLastIndex = -1;
1115 
1116  for (NodeVecType::const_iterator n = Nodes.begin(); n != Nodes.end(); ++n) {
1117  integer iIndex = (*n)->iGetFirstIndex();
1118  integer iNumDofs = (*n)->iGetNumDof();
1119 
1120  if (iFirstIndex > iIndex) {
1121  iFirstIndex = iIndex;
1122  }
1123 
1124  if (iLastIndex < iIndex + iNumDofs) {
1125  iLastIndex = iIndex + iNumDofs;
1126  }
1127  }
1128 
1129  silent_cout("DataManager::IDSetTest(): SolTest range=[" << iFirstIndex + 1 << ":" << iLastIndex << "]" << std::endl);
1130 
1131  pSolTest->SetRange(iFirstIndex + 1, iLastIndex);
1132 
1133  iFirstIndex = std::numeric_limits<integer>::max();
1134  iLastIndex = -1;
1135 
1136  for (ElemContainerType::const_iterator j = ElemData[Elem::JOINT].ElemContainer.begin();
1137  j != ElemData[Elem::JOINT].ElemContainer.end(); ++j)
1138  {
1139  const Joint *pJ = Cast<Joint>(j->second);
1140  if (!pJ->bIsTorque() && !pJ->bIsPrescribedMotion()) {
1141  continue;
1142  }
1143 
1144  const ElemWithDofs* pEWD = Cast<ElemWithDofs>(j->second);
1145  integer iNumDofs = pEWD->iGetNumDof();
1146  if (iNumDofs > 0) {
1147  integer iIndex = pEWD->iGetFirstIndex();
1148 
1149  if (iFirstIndex > iIndex) {
1150  iFirstIndex = iIndex;
1151  }
1152 
1153  if (iLastIndex < iIndex + iNumDofs) {
1154  iLastIndex = iIndex + iNumDofs;
1155  }
1156  }
1157  }
1158 
1159  if (bFullResTest) {
1160  // NOTE: assumes *ALL* residual elements need to be evaluated! Use with care...
1161  iFirstIndex = 0;
1162  }
1163 
1164  silent_cout("DataManager::IDSetTest(): ResTest range=[" << iFirstIndex + 1 << ":" << iLastIndex << "]" << std::endl);
1165 
1166  pResTest->SetRange(iFirstIndex + 1, iLastIndex);
1167 }
DataManager::iTotDofOwners
integer iTotDofOwners
Definition: dataman.h:795
StructNode::GetWPrev
virtual const Vec3 & GetWPrev(void) const
Definition: strnode.h:1018
DataManager::IDSetTest
virtual void IDSetTest(NonlinearSolverTestRange *pResTest, NonlinearSolverTestRange *pSolTest, bool bFullResTest)
Definition: invdataman.cc:1111
DataManager::Elems
ElemVecType Elems
Definition: dataman.h:609
DataManager::ElemDataStructure::ElemContainer
ElemContainerType ElemContainer
Definition: dataman.h:591
Vec3::Cross
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
body.h
MBDYN_EXCEPT_ARGS
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
Body
Definition: body.h:270
Vec3
Definition: matvec3.h:98
DataManager::ElemDataStructure::DofOwnerType
DofOwner::Type DofOwnerType
Definition: dataman.h:565
VecIter::bGetNext
bool bGetNext(T &TReturn) const
Definition: veciter.h:118
DEBUGCOUTFNAME
#define DEBUGCOUTFNAME(fname)
Definition: myassert.h:256
DofOwner::iNumDofs
unsigned int iNumDofs
Definition: dofown.h:95
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
DataManager::Dofs
DofVecType Dofs
Definition: dataman.h:813
DataManager::iIDNodeTotNumDofs
int iIDNodeTotNumDofs
Definition: dataman.h:134
Elem::bIsErgonomy
bool bIsErgonomy(void) const
Definition: elem.cc:83
DataManager::Nodes
NodeVecType Nodes
Definition: dataman.h:743
Beam2
Definition: beam2.h:50
StructDispNode::GetXPrev
virtual const Vec3 & GetXPrev(void) const
Definition: strnode.h:304
Elem::bIsRightHandSide
bool bIsRightHandSide(void) const
Definition: elem.cc:89
DataManager::pWorkVec
MySubVectorHandler * pWorkVec
Definition: dataman.h:633
Body::pGetNode
const StructNode * pGetNode(void) const
Definition: body.cc:818
Body::GetS
Vec3 GetS(void) const
Definition: body.cc:804
VariableSubMatrixHandler::AddToT
MatrixHandler & AddToT(MatrixHandler &MH) const
Definition: submat.h:1277
DataManager::AssRes
virtual void AssRes(VectorHandler &ResHdl, doublereal dCoef)
Definition: elman.cc:498
DEBUGCERR
#define DEBUGCERR(msg)
Definition: myassert.h:235
Elem::GetElemType
virtual Elem::Type GetElemType(void) const =0
DataManager::iIDJointTotNumDofs
int iIDJointTotNumDofs
Definition: dataman.h:135
DofOwner::iFirstIndex
integer iFirstIndex
Definition: dofown.h:94
DataManager::DofOwners
std::vector< DofOwner > DofOwners
Definition: dataman.h:796
InverseSolver::GetProblemType
InverseDynamics::Type GetProblemType(void) const
Definition: invsolver.cc:889
DataManager::NodeDataStructure::NodeContainer
NodeContainerType NodeContainer
Definition: dataman.h:731
RotManip::VecRot
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
DataManager::iTotDofs
integer iTotDofs
Definition: dataman.h:809
MatrixHandler::Reset
virtual void Reset(void)=0
Joint::bIsPrescribedMotion
bool bIsPrescribedMotion(void) const
Definition: joint.cc:170
DataManager::IDAfterConvergence
virtual void IDAfterConvergence(void) const
Definition: invdataman.cc:834
DofOwnerOwner::pGetDofOwner
virtual const DofOwner * pGetDofOwner(void) const
Definition: dofown.h:113
NonlinearSolverTestRange::SetRange
void SetRange(integer iFirstIndex, integer iLastIndex)
Definition: nonlin.cc:406
DataManager::pXPrimePrimeCurr
VectorHandler * pXPrimePrimeCurr
Definition: dataman.h:112
DataManager::IDDofOwnerSet
void IDDofOwnerSet(void)
Definition: invdataman.cc:852
DataManager::LinkToSolution
void LinkToSolution(VectorHandler &XCurr, VectorHandler &XPrimeCurr)
Definition: dataman2.cc:172
Solver::pGetStepIntegrator
virtual StepIntegrator * pGetStepIntegrator(void) const
Definition: solver.h:407
InverseDynamicsStepSolver::GetOrder
InverseDynamics::Order GetOrder(void) const
Definition: stepsol.cc:1516
DataManager::IDDofInit
void IDDofInit(void)
Definition: invdataman.cc:895
DataManager::iIDGetJointTotNumDofs
int iIDGetJointTotNumDofs(void) const
Definition: invdataman.cc:1086
DataManager::pSolver
Solver * pSolver
Definition: dataman.h:99
StructNode::GetRPrev
virtual const Mat3x3 & GetRPrev(void) const
Definition: strnode.h:1000
DriveHandler::dGetTimeStep
doublereal dGetTimeStep(void) const
Definition: drive.h:393
DataManager::NodeData
struct DataManager::NodeDataStructure NodeData[Node::LASTNODETYPE]
StructDispNode::GetVPrev
virtual const Vec3 & GetVPrev(void) const
Definition: strnode.h:316
DEBUGCOUT
#define DEBUGCOUT(msg)
Definition: myassert.h:232
DataManager::iIDGetNodeTotNumDofs
int iIDGetNodeTotNumDofs(void) const
Definition: invdataman.cc:1080
DataManager::pXPrimeCurr
VectorHandler * pXPrimeCurr
Definition: dataman.h:109
StructNode::GetWCurr
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
Body::dGetM
doublereal dGetM(void) const
Definition: body.cc:797
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
DataManager::ElemIter
VecIter< Elem * > ElemIter
Definition: dataman.h:612
Iter
Definition: veciter.h:52
DataManager::pLambdaCurr
VectorHandler * pLambdaCurr
Definition: dataman.h:113
DataManager::AssConstrJac
virtual void AssConstrJac(MatrixHandler &JacHdl)
Definition: invdataman.cc:94
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
Elem::iGetNumDof
virtual unsigned int iGetNumDof(void) const
Definition: elem.cc:118
SimulationEntity::iGetNumDof
virtual unsigned int iGetNumDof(void) const =0
DriveHandler::LinkToSolution
void LinkToSolution(const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: drive.cc:260
Elem
Definition: elem.h:75
Joint::bIsTorque
bool bIsTorque(void) const
Definition: joint.cc:176
Elem::Type
Type
Definition: elem.h:91
psElemNames
const char * psElemNames[]
Definition: enums.cc:39
SimulationEntity::AfterConvergence
virtual void AfterConvergence(const VectorHandler &X, const VectorHandler &XP)
Definition: simentity.cc:120
Elem::AssRes
virtual SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)=0
StructDispNode::GetVCurr
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
DataManager::Update
virtual void Update(void) const
Definition: dataman2.cc:2511
Joint
Definition: joint.h:50
VecIter::bGetFirst
bool bGetFirst(T &TReturn) const
Definition: veciter.h:88
Mat3x3::MulMT
Mat3x3 MulMT(const Mat3x3 &m) const
Definition: matvec3.cc:444
StructNode::GetWPPrev
virtual const Vec3 & GetWPPrev(void) const
Definition: strnode.h:1036
DataManager::DrvHdl
DriveHandler DrvHdl
Definition: dataman.h:104
StructDispNode::GetXPPPrev
virtual const Vec3 & GetXPPPrev(void) const
Definition: strnode.h:328
DataManager::DofData
struct DataManager::@30 DofData[DofOwner::LASTDOFTYPE]
DataManager::AssConstrRes
virtual void AssConstrRes(VectorHandler &ResHdl, InverseDynamics::Order iOrder)
Definition: invdataman.cc:303
DofOwnerOwner::iGetFirstIndex
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
doublereal
double doublereal
Definition: colamd.c:52
dataman_.h
InverseSolver::GetWeight
void GetWeight(InverseDynamics::Order iOrder, doublereal &dw1, doublereal &dw2) const
Definition: invsolver.cc:895
integer
long int integer
Definition: colamd.c:51
Beam2::AssJac
virtual VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: beam2.cc:485
WithLabel::GetLabel
unsigned int GetLabel(void) const
Definition: withlab.cc:62
DataManager::ElemData
struct DataManager::ElemDataStructure ElemData[Elem::LASTELEMTYPE]
DEBUGLCOUT
#define DEBUGLCOUT(level, msg)
Definition: myassert.h:244
Body::GetJ
Mat3x3 GetJ(void) const
Definition: body.cc:811
DataManager::pWorkMat
VariableSubMatrixHandler * pWorkMat
Definition: dataman.h:632
Elem::AssJac
virtual VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)=0
DataManager::bOutputAccels
bool bOutputAccels
Definition: dataman.h:119
DataManager::pXCurr
VectorHandler * pXCurr
Definition: dataman.h:108
DataManager::iIDGetTotNumDofs
int iIDGetTotNumDofs(void) const
Definition: invdataman.cc:1092