1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/modules/module-loadinc/module-loadinc.cc,v 1.19 2017/01/12 14:54:26 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 *
10 * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
11 * via La Masa, 34 - 20156 Milano, Italy
12 * http://www.aero.polimi.it
13 *
14 * Changing this copyright notice is forbidden.
15 *
16 * This program is free software; you can redistribute it and/or modify
17 * it under the terms of the GNU General Public License as published by
18 * the Free Software Foundation (version 2 of the License).
19 *
20 *
21 * This program is distributed in the hope that it will be useful,
22 * but WITHOUT ANY WARRANTY; without even the implied warranty of
23 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
24 * GNU General Public License for more details.
25 *
26 * You should have received a copy of the GNU General Public License
27 * along with this program; if not, write to the Free Software
28 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
29 */
30 /*
31 * Authors: Yang Ding <dingyang@gatech.edu>
32 * Pierangelo Masarati <masarati@aero.polimi.it>
33 */
34
35 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
36
37 #include <iostream>
38 #include <cfloat>
39 #include <vector>
40
41 #include "solver.h"
42 #include "dataman.h"
43 #include "userelem.h"
44 #include "driven.h"
45 #include "Rot.hh"
46
47 class LoadIncNorm
48 : virtual public Elem, public UserDefinedElem {
49 private:
50 int m_FirstSteps;
51 doublereal m_dP;
52 doublereal m_dPPrev;
53 DriveOwner m_DeltaS;
54 doublereal m_dDeltaS;
55 doublereal m_dS;
56
57
58 // stop at or past max load
59 doublereal m_dPMax;
60
61 // TODO: implement other strategies?
62
63 // TODO: handle StructDispNode
64 struct NodeData {
65 const StructDispNode *pNode;
66 Vec3 DX;
67 Vec3 DTheta;
68 };
69 std::vector<NodeData> m_Nodes;
70 doublereal m_dCompliance;
71 doublereal m_dRefLen;
72 // integer m_iDofOffset;
73 unsigned m_iDim;
74 doublereal m_dDeltaP;
75
76 doublereal m_DeltaS2;
77
78 public:
79 LoadIncNorm(unsigned uLabel, const DofOwner *pDO,
80 DataManager* pDM, MBDynParser& HP);
81 virtual ~LoadIncNorm(void);
82
83 virtual void Output(OutputHandler& OH) const;
84 virtual void WorkSpaceDim(integer* piNumRows, integer* piNumCols) const;
85 VariableSubMatrixHandler&
86 AssJac(VariableSubMatrixHandler& WorkMat,
87 doublereal dCoef,
88 const VectorHandler& XCurr,
89 const VectorHandler& XPrimeCurr);
90 SubVectorHandler&
91 AssRes(SubVectorHandler& WorkVec,
92 doublereal dCoef,
93 const VectorHandler& XCurr,
94 const VectorHandler& XPrimeCurr);
95 unsigned int iGetNumPrivData(void) const;
96 unsigned int iGetPrivDataIdx(const char *s) const;
97 doublereal dGetPrivData(unsigned int i) const;
98 int iGetNumConnectedNodes(void) const;
99 void GetConnectedNodes(std::vector<const Node *>& connectedNodes) const;
100 void SetValue(DataManager *pDM, VectorHandler& X, VectorHandler& XP,
101 SimulationEntity::Hints *ph);
102 virtual unsigned int iGetNumDof(void) const;
103 virtual DofOrder::Order GetDofType(unsigned int i) const;
104 virtual DofOrder::Order GetEqType(unsigned int i) const;
105 std::ostream& Restart(std::ostream& out) const;
106 virtual unsigned int iGetInitialNumDof(void) const;
107 virtual void
108 InitialWorkSpaceDim(integer* piNumRows, integer* piNumCols) const;
109 VariableSubMatrixHandler&
110 InitialAssJac(VariableSubMatrixHandler& WorkMat,
111 const VectorHandler& XCurr);
112 SubVectorHandler&
113 InitialAssRes(SubVectorHandler& WorkVec, const VectorHandler& XCurr);
114 virtual void AfterPredict(VectorHandler& X, VectorHandler& XP);
115 virtual void AfterConvergence(const VectorHandler& X,
116 const VectorHandler& XP);
117
118 // get private variable
119 doublereal dGetP(void) const;
120 };
121
LoadIncNorm(unsigned uLabel,const DofOwner * pDO,DataManager * pDM,MBDynParser & HP)122 LoadIncNorm::LoadIncNorm(
123 unsigned uLabel, const DofOwner *pDO,
124 DataManager* pDM, MBDynParser& HP)
125 : Elem(uLabel, flag(0)),
126 UserDefinedElem(uLabel, pDO),
127 m_FirstSteps(2),
128 m_dS(0.),
129 m_dPMax(1.),
130 m_dCompliance(1.),
131 m_dRefLen(1.),
132 m_iDim(0)
133 {
134 // help
135 if (HP.IsKeyWord("help")) {
136 silent_cout(
137 "\n"
138 "Module: loadinc - load increment normalization\n"
139 "Author: Pierangelo Masarati <masarati@aero.polimi.it>\n"
140 "Organization: Dipartimento di Ingegneria Aerospaziale\n"
141 " Politecnico di Milano\n"
142 " http://www.aero.polimi.it/\n"
143 "\n"
144 " All rights reserved\n"
145 "\n"
146 "Usage:\n"
147 "\n"
148 "user defined : <label> , load increment normalization ,\n"
149 " [ max load , <max_load> , ] # bails out when p >= max_load\n"
150 " [ compliance , <compliance> , ] # compliance*p = length\n"
151 " [ reference length, <ref_length> , ] # multiplies DeltaTheta\n"
152 " (DriveCaller) <DeltaS> ; # arc length increment\n"
153 "\n"
154 "# output:\n"
155 "# 1: <label>\n"
156 "# 2: <p>\n"
157 "# 3: <s>\n"
158 << std::endl);
159
160 if (!HP.IsArg()) {
161 /*
162 * Exit quietly if nothing else is provided
163 */
164 throw NoErr(MBDYN_EXCEPT_ARGS);
165 }
166 }
167
168 if (HP.IsKeyWord("max" "load")) {
169 m_dPMax = HP.GetReal();
170 if (m_dPMax <= 0.) {
171 silent_cerr("LoadIncNorm(" << uLabel << "): invalid \"max load\" at line " << HP.GetLineData() << std::endl);
172 throw NoErr(MBDYN_EXCEPT_ARGS);
173 }
174 }
175
176 if (HP.IsKeyWord("compliance")) {
177 m_dCompliance = HP.GetReal();
178 if (m_dCompliance <= std::numeric_limits<doublereal>::epsilon()) {
179 silent_cerr("LoadIncNorm(" << uLabel << "): invalid \"compliance\" at line " << HP.GetLineData() << std::endl);
180 throw NoErr(MBDYN_EXCEPT_ARGS);
181 }
182 }
183
184 if (HP.IsKeyWord("reference" "length")) {
185 m_dRefLen = HP.GetReal();
186 if (m_dRefLen < 0.) {
187 silent_cerr("LoadIncNorm(" << uLabel << "): invalid \"reference length\" at line " << HP.GetLineData() << std::endl);
188 throw NoErr(MBDYN_EXCEPT_ARGS);
189 }
190
191 #if 0
192 if (m_dRefLen == 0.) {
193 m_iDofOffset = 3;
194 }
195 #endif
196 }
197
198 DriveCaller *pDC = HP.GetDriveCaller();
199 m_DeltaS.Set(pDC);
200
201 SetOutputFlag(pDM->fReadOutput(HP, Elem::LOADABLE));
202
203 // initialize data structures
204 unsigned uCnt;
205 DataManager::NodeContainerType::const_iterator i;
206 for (i = pDM->begin(Node::STRUCTURAL), uCnt = 0; i != pDM->end(Node::STRUCTURAL); ++i) {
207 const StructDispNode *pDNode(dynamic_cast<const StructDispNode *>(i->second));
208 ASSERT(pDNode != 0);
209 const StructNode *pNode(dynamic_cast<const StructNode *>(pDNode));
210 if (pNode != 0 && pNode->GetStructNodeType() == StructNode::DUMMY) {
211 continue;
212 }
213 uCnt++;
214 }
215
216 m_Nodes.resize(uCnt);
217
218 for (i = pDM->begin(Node::STRUCTURAL), uCnt = 0; i != pDM->end(Node::STRUCTURAL); ++i) {
219 const StructDispNode *pDNode(dynamic_cast<const StructDispNode *>(i->second));
220 ASSERT(pDNode != 0);
221 const StructNode *pNode(dynamic_cast<const StructNode *>(pDNode));
222 if (pNode != 0 && pNode->GetStructNodeType() == StructNode::DUMMY) {
223 continue;
224 }
225 m_Nodes[uCnt].pNode = pDNode;
226 if (pNode != 0 && m_dRefLen > 0.) {
227 m_iDim += 6;
228
229 } else {
230 m_iDim += 3;
231 }
232 uCnt++;
233 }
234
235 m_dDeltaS = m_DeltaS.dGet();
236 if (m_dDeltaS < 0.) {
237 silent_cerr("LoadIncNorm(" << uLabel << "): DeltaS must be positive" << std::endl);
238 throw NoErr(MBDYN_EXCEPT_ARGS);
239 }
240 m_dDeltaP = m_dDeltaS/m_dCompliance;
241 m_dPPrev = -m_dDeltaP;
242
243 m_DeltaS2 = m_dDeltaS;
244
245 // std::cerr << "### " << __PRETTY_FUNCTION__ << " m_dP=" << m_dP << " m_dDeltaP=" << m_dDeltaP << " m_dPPrev=" << m_dPPrev << std::endl;
246 }
247
~LoadIncNorm(void)248 LoadIncNorm::~LoadIncNorm(void)
249 {
250 NO_OP;
251 }
252
253 void
Output(OutputHandler & OH) const254 LoadIncNorm::Output(OutputHandler& OH) const
255 {
256 if (bToBeOutput()) {
257 std::ostream& out = OH.Loadable();
258
259 out << GetLabel()
260 << " " << m_dP
261 << " " << m_dS
262 << std::endl;
263 }
264 }
265
266 void
AfterPredict(VectorHandler & X,VectorHandler & XP)267 LoadIncNorm::AfterPredict(VectorHandler& X, VectorHandler& XP)
268 {
269 NO_OP;
270 }
271
272 void
AfterConvergence(const VectorHandler & X,const VectorHandler & XP)273 LoadIncNorm::AfterConvergence(const VectorHandler& X,
274 const VectorHandler& XP)
275 {
276 if (m_dP >= m_dPMax) {
277 mbdyn_set_stop_at_end_of_time_step();
278 }
279
280 // std::cerr << "### " << __PRETTY_FUNCTION__ << " m_FirstSteps=" << m_FirstSteps << std::endl;
281
282 if (m_FirstSteps) {
283 m_FirstSteps--;
284 }
285
286 m_dS += m_DeltaS.dGet();
287 m_dPPrev = m_dP;
288
289 // std::cerr << "### " << __PRETTY_FUNCTION__ << " m_dP=" << m_dP << " m_dDeltaP=" << m_dDeltaP << " m_dPPrev=" << m_dPPrev << std::endl;
290 }
291
292 void
WorkSpaceDim(integer * piNumRows,integer * piNumCols) const293 LoadIncNorm::WorkSpaceDim(integer* piNumRows, integer* piNumCols) const
294 {
295 *piNumRows = 1;
296 *piNumCols = m_iDim + 1;
297 }
298
299 VariableSubMatrixHandler&
AssJac(VariableSubMatrixHandler & WorkMat,doublereal dCoef,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)300 LoadIncNorm::AssJac(VariableSubMatrixHandler& WorkMat,
301 doublereal dCoef,
302 const VectorHandler& XCurr,
303 const VectorHandler& XPrimeCurr)
304 {
305 FullSubMatrixHandler& WM = WorkMat.SetFull();
306
307 integer iIndex = iGetFirstIndex() + 1;
308
309 // std::cerr << "### " << __PRETTY_FUNCTION__ << " m_FirstSteps=" << m_FirstSteps << std::endl;
310
311 if (m_FirstSteps || m_dDeltaS == 0.) {
312 WM.ResizeReset(1, 1);
313 WM.PutRowIndex(1, iIndex);
314 WM.PutColIndex(1, iIndex);
315 WM.PutCoef(1, 1, m_dCompliance);
316
317 } else {
318 integer iNumRows, iNumCols;
319 WorkSpaceDim(&iNumRows, &iNumCols);
320 WM.ResizeReset(iNumRows, iNumCols);
321
322 WM.PutRowIndex(1, iIndex);
323 WM.PutColIndex(iNumCols, iIndex);
324 doublereal d = (m_dDeltaP*m_dCompliance*m_dCompliance)/m_DeltaS2;
325 if (std::abs(d) <= 10.*std::numeric_limits<doublereal>::epsilon()) {
326 d = m_DeltaS.dGet()*m_dCompliance;
327 }
328 WM.PutCoef(1, iNumCols, d);
329
330
331 // std::cerr << "### " << __PRETTY_FUNCTION__ << " m_dP=" << m_dP << " m_dDeltaP=" << m_dDeltaP << " m_dPPrev=" << m_dPPrev << std::endl;
332
333 doublereal dCoefX = dCoef/m_DeltaS2;
334 doublereal dCoefTheta = dCoefX*m_dRefLen*m_dRefLen;
335
336 integer iNodeOffset = 0;
337 for (std::vector<NodeData>::const_iterator i = m_Nodes.begin(); i != m_Nodes.end(); ++i) {
338 integer iFirstPositionIndex = i->pNode->iGetFirstIndex();
339
340 for (integer iCnt = 1; iCnt <= 3; iCnt++) {
341 WM.PutColIndex(iNodeOffset + iCnt, iFirstPositionIndex + iCnt);
342 WM.PutCoef(1, iNodeOffset + iCnt, dCoefX*i->DX(iCnt));
343 }
344
345 if (m_dRefLen > 0.) {
346 const StructNode *pNode(dynamic_cast<const StructNode *>(i->pNode));
347 if (pNode != 0) {
348 // FIXME: check linearization
349 Mat3x3 Gm1 = RotManip::DRot_I(i->DTheta);
350 Vec3 VTmp = Gm1.MulTV(i->DTheta);
351
352 for (integer iCnt = 1; iCnt <= 3; iCnt++) {
353 WM.PutColIndex(iNodeOffset + 3 + iCnt, iFirstPositionIndex + 3 + iCnt);
354 WM.PutCoef(1, iNodeOffset + 3 + iCnt, dCoefTheta*VTmp(iCnt));
355 }
356 iNodeOffset += 6;
357
358 } else {
359 iNodeOffset += 3;
360 }
361
362 } else {
363 iNodeOffset += 3;
364 }
365 }
366 }
367
368 return WorkMat;
369 }
370
371 SubVectorHandler&
AssRes(SubVectorHandler & WorkVec,doublereal dCoef,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)372 LoadIncNorm::AssRes(SubVectorHandler& WorkVec,
373 doublereal dCoef,
374 const VectorHandler& XCurr,
375 const VectorHandler& XPrimeCurr)
376 {
377 integer iNumRows, iNumCols;
378 WorkSpaceDim(&iNumRows, &iNumCols);
379 WorkVec.ResizeReset(iNumRows);
380
381 integer iIndex = iGetFirstIndex() + 1;
382
383 m_dP = XCurr(iIndex);
384 m_dDeltaP = m_dP - m_dPPrev;
385
386 // std::cerr << "### " << __PRETTY_FUNCTION__ << " m_dP=" << m_dP << " m_dDeltaP=" << m_dDeltaP << " m_dPPrev=" << m_dPPrev << std::endl;
387
388 m_dDeltaS = m_DeltaS.dGet();
389 if (m_dDeltaS < 0.) {
390 silent_cerr("LoadIncNorm(" << uLabel << ")::AssRes(): DeltaS must be positive" << std::endl);
391 throw NoErr(MBDYN_EXCEPT_ARGS);
392 }
393 doublereal d;
394
395 // std::cerr << "### " << __PRETTY_FUNCTION__ << " m_FirstSteps=" << m_FirstSteps << std::endl;
396
397 if (m_FirstSteps == 2) {
398 d = -m_dP*m_dCompliance;
399
400 } else if (m_FirstSteps == 1 || m_dDeltaS == 0.) {
401 d = m_dDeltaS - m_dP*m_dCompliance;
402
403 } else {
404 d = 0.;
405
406 for (std::vector<NodeData>::iterator i = m_Nodes.begin(); i != m_Nodes.end(); ++i) {
407 i->DX = i->pNode->GetXCurr() - i->pNode->GetXPrev();
408 d += i->DX.Dot();
409
410 if (m_dRefLen > 0.) {
411 const StructNode *pNode(dynamic_cast<const StructNode *>(i->pNode));
412 if (pNode != 0) {
413 i->DTheta = RotManip::VecRot(pNode->GetRCurr().MulMT(pNode->GetRPrev()));
414 d += (m_dRefLen*m_dRefLen)*i->DTheta.Dot();
415 }
416 }
417 }
418
419 d += (m_dDeltaP*m_dCompliance)*(m_dDeltaP*m_dCompliance);
420 m_DeltaS2 = std::sqrt(d);
421
422 d = m_dDeltaS - m_DeltaS2;
423 }
424
425 WorkVec.PutItem(1, iIndex, d);
426
427 return WorkVec;
428 }
429
430 unsigned int
iGetNumPrivData(void) const431 LoadIncNorm::iGetNumPrivData(void) const
432 {
433 return 3;
434 }
435
436 unsigned int
iGetPrivDataIdx(const char * s) const437 LoadIncNorm::iGetPrivDataIdx(const char *s) const
438 {
439 if (strcmp(s, "p") == 0) {
440 return 1;
441 }
442
443 if (strcmp(s, "S") == 0) {
444 return 2;
445 }
446
447 if (strcmp(s, "DeltaS") == 0) {
448 return 3;
449 }
450
451 return 0;
452 }
453
454 doublereal
dGetPrivData(unsigned int i) const455 LoadIncNorm::dGetPrivData(unsigned int i) const
456 {
457 switch (i) {
458 case 1:
459 return m_dP;
460
461 case 2:
462 return m_dS;
463
464 case 3:
465 return m_dDeltaS;
466
467 default:
468 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
469 }
470 }
471
472 int
iGetNumConnectedNodes(void) const473 LoadIncNorm::iGetNumConnectedNodes(void) const
474 {
475 return m_Nodes.size();
476 }
477
478 void
GetConnectedNodes(std::vector<const Node * > & connectedNodes) const479 LoadIncNorm::GetConnectedNodes(std::vector<const Node *>& connectedNodes) const
480 {
481 connectedNodes.resize(m_Nodes.size());
482
483 for (unsigned uCnt = 0; uCnt < m_Nodes.size(); uCnt++) {
484 connectedNodes[uCnt] = m_Nodes[uCnt].pNode;
485 }
486 }
487
488 void
SetValue(DataManager * pDM,VectorHandler & X,VectorHandler & XP,SimulationEntity::Hints * ph)489 LoadIncNorm::SetValue(DataManager *pDM,
490 VectorHandler& X, VectorHandler& XP,
491 SimulationEntity::Hints *ph)
492 {
493 integer iIndex = iGetFirstIndex() + 1;
494 X(iIndex) = 0.;
495 XP(iIndex) = m_dPPrev;
496 }
497
498 unsigned int
iGetNumDof(void) const499 LoadIncNorm::iGetNumDof(void) const
500 {
501 return 1;
502 }
503
504 DofOrder::Order
GetDofType(unsigned int i) const505 LoadIncNorm::GetDofType(unsigned int i) const
506 {
507 ASSERT(i == 0);
508 return DofOrder::ALGEBRAIC;
509 }
510
511 DofOrder::Order
GetEqType(unsigned int i) const512 LoadIncNorm::GetEqType(unsigned int i) const
513 {
514 ASSERT(i == 0);
515 return DofOrder::ALGEBRAIC;
516 }
517
518 std::ostream&
Restart(std::ostream & out) const519 LoadIncNorm::Restart(std::ostream& out) const
520 {
521 return out << "# LoadIncNorm: not implemented" << std::endl;
522 }
523
524 unsigned int
iGetInitialNumDof(void) const525 LoadIncNorm::iGetInitialNumDof(void) const
526 {
527 return 0;
528 }
529
530 void
InitialWorkSpaceDim(integer * piNumRows,integer * piNumCols) const531 LoadIncNorm::InitialWorkSpaceDim(
532 integer* piNumRows,
533 integer* piNumCols) const
534 {
535 *piNumRows = 0;
536 *piNumCols = 0;
537 }
538
539 VariableSubMatrixHandler&
InitialAssJac(VariableSubMatrixHandler & WorkMat,const VectorHandler & XCurr)540 LoadIncNorm::InitialAssJac(
541 VariableSubMatrixHandler& WorkMat,
542 const VectorHandler& XCurr)
543 {
544 // should not be called, since initial workspace is empty
545 ASSERT(0);
546
547 WorkMat.SetNullMatrix();
548
549 return WorkMat;
550 }
551
552 SubVectorHandler&
InitialAssRes(SubVectorHandler & WorkVec,const VectorHandler & XCurr)553 LoadIncNorm::InitialAssRes(
554 SubVectorHandler& WorkVec,
555 const VectorHandler& XCurr)
556 {
557 // should not be called, since initial workspace is empty
558 ASSERT(0);
559
560 WorkVec.ResizeReset(0);
561
562 return WorkVec;
563 }
564
565 doublereal
dGetP(void) const566 LoadIncNorm::dGetP(void) const
567 {
568 return m_dP;
569 }
570
571 class LoadIncForce
572 : virtual public Elem, public UserDefinedElem {
573 private:
574 bool m_bCouple;
575 bool m_bFollower;
576
577 const LoadIncNorm *m_pLoadIncNorm;
578 const DrivenElem *m_pDrivenLoadIncNorm;
579 const StructDispNode *m_pDNode;
580 const StructNode *m_pNode;
581 Vec3 m_o;
582 Vec3 m_Dir;
583 Vec3 m_F;
584 Vec3 m_M;
585
586 public:
587 LoadIncForce(unsigned uLabel, const DofOwner *pDO,
588 DataManager* pDM, MBDynParser& HP);
589 virtual ~LoadIncForce(void);
590
591 virtual void Output(OutputHandler& OH) const;
592 virtual void WorkSpaceDim(integer* piNumRows, integer* piNumCols) const;
593 VariableSubMatrixHandler&
594 AssJac(VariableSubMatrixHandler& WorkMat,
595 doublereal dCoef,
596 const VectorHandler& XCurr,
597 const VectorHandler& XPrimeCurr);
598 SubVectorHandler&
599 AssRes(SubVectorHandler& WorkVec,
600 doublereal dCoef,
601 const VectorHandler& XCurr,
602 const VectorHandler& XPrimeCurr);
603 unsigned int iGetNumPrivData(void) const;
604 int iGetNumConnectedNodes(void) const;
605 void GetConnectedNodes(std::vector<const Node *>& connectedNodes) const;
606 void SetValue(DataManager *pDM, VectorHandler& X, VectorHandler& XP,
607 SimulationEntity::Hints *ph);
608 std::ostream& Restart(std::ostream& out) const;
609 virtual unsigned int iGetInitialNumDof(void) const;
610 virtual void
611 InitialWorkSpaceDim(integer* piNumRows, integer* piNumCols) const;
612 VariableSubMatrixHandler&
613 InitialAssJac(VariableSubMatrixHandler& WorkMat,
614 const VectorHandler& XCurr);
615 SubVectorHandler&
616 InitialAssRes(SubVectorHandler& WorkVec, const VectorHandler& XCurr);
617 virtual void AfterPredict(VectorHandler& X, VectorHandler& XP);
618 };
619
LoadIncForce(unsigned uLabel,const DofOwner * pDO,DataManager * pDM,MBDynParser & HP)620 LoadIncForce::LoadIncForce(
621 unsigned uLabel, const DofOwner *pDO,
622 DataManager* pDM, MBDynParser& HP)
623 : Elem(uLabel, flag(0)),
624 UserDefinedElem(uLabel, pDO),
625 m_pDrivenLoadIncNorm(0)
626 {
627 // help
628 if (HP.IsKeyWord("help")) {
629 silent_cout(
630 "\n"
631 "Module: loadinc - load increment force\n"
632 "Author: Pierangelo Masarati <masarati@aero.polimi.it>\n"
633 "Organization: Dipartimento di Ingegneria Aerospaziale\n"
634 " Politecnico di Milano\n"
635 " http://www.aero.polimi.it/\n"
636 "\n"
637 " All rights reserved\n"
638 "\n"
639 "Usage:\n"
640 "\n"
641 "user defined : <label> , load increment force ,\n"
642 " { force | couple } ,\n"
643 " { absolute | follower } ,\n"
644 " <node_label> ,\n"
645 " [ position , (Vec3) <position> , ] # meaningless for couple\n"
646 " (Vec3) <direction> , \n"
647 " load increment normalization , <lin_label> ;\n"
648 "\n"
649 "# output:\n"
650 "# 1: <label>@<node_label>\n"
651 "# if type ::= \"force\":\n"
652 "# 2,3,4: fx fy fz\n"
653 "# if node is not displacement-only:\n"
654 "# 5,6,7: mx my mz\n"
655 "# else\n"
656 "# 2,3,4: mx my mz\n"
657 << std::endl);
658
659 if (!HP.IsArg()) {
660 /*
661 * Exit quietly if nothing else is provided
662 */
663 throw NoErr(MBDYN_EXCEPT_ARGS);
664 }
665 }
666
667 if (HP.IsKeyWord("force")) {
668 m_bCouple = false;
669
670 } else if (HP.IsKeyWord("couple")) {
671 m_bCouple = true;
672
673 } else {
674 silent_cerr("LoadIncForce(" << uLabel << "): missing { force | couple } or unexpected type at line " << HP.GetLineData() << std::endl);
675 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
676 }
677
678 if (HP.IsKeyWord("absolute")) {
679 m_bFollower = false;
680
681 } else if (HP.IsKeyWord("follower")) {
682 m_bFollower = true;
683
684 } else {
685 silent_cerr("LoadIncForce(" << uLabel << "): missing { absolute | follower } or unexpected type at line " << HP.GetLineData() << std::endl);
686 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
687 }
688
689 m_pDNode = pDM->ReadNode<const StructDispNode, Node::STRUCTURAL>(HP);
690 ASSERT(m_pDNode != 0);
691 m_pNode = dynamic_cast<const StructNode *>(m_pDNode);
692
693 m_o = ::Zero3;
694 if (m_pNode != 0) {
695 ReferenceFrame rf(m_pNode);
696
697 if (HP.IsKeyWord("position")) {
698 m_o = HP.GetPosRel(rf);
699 }
700
701 if (m_bFollower) {
702 m_Dir = HP.GetVecRel(rf);
703
704 } else {
705 m_Dir = HP.GetVecAbs(rf);
706 }
707
708 } else {
709 m_Dir = HP.GetVec3();
710 }
711
712 if (m_Dir.IsNull()) {
713 silent_cerr("LoadIncForce(" << uLabel << "): null direction at line " << HP.GetLineData() << std::endl);
714 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
715 }
716
717 if (!HP.IsKeyWord("load" "increment" "normalization")) {
718 silent_cerr("LoadIncForce(" << uLabel << "): missing \"load increment normalization\" keyword at line " << HP.GetLineData() << std::endl);
719 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
720 }
721
722 // if m_pLoadIncNorm is driven, make sure the LoadIncForce
723 // and the LoadIncNorm are simultaneously active
724 Elem *pEl = pDM->ReadElem(HP, Elem::LOADABLE);
725 m_pLoadIncNorm = dynamic_cast<const LoadIncNorm *>(pEl);
726 if (m_pLoadIncNorm == 0) {
727 m_pDrivenLoadIncNorm = dynamic_cast<const DrivenElem *>(pEl);
728 if (m_pDrivenLoadIncNorm == 0) {
729 silent_cerr("LoadIncForce(" << uLabel << "): invalid \"load increment normalization\" UseDefined(" << pEl->GetLabel() << ") element at line " << HP.GetLineData() << std::endl);
730 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
731 }
732
733 m_pLoadIncNorm = dynamic_cast<const LoadIncNorm *>(m_pDrivenLoadIncNorm->pGetElem());
734 if (m_pLoadIncNorm == 0) {
735 silent_cerr("LoadIncForce(" << uLabel << "): invalid \"load increment normalization\" UseDefined(" << pEl->GetLabel() << ") element at line " << HP.GetLineData() << std::endl);
736 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
737 }
738 }
739
740 SetOutputFlag(pDM->fReadOutput(HP, Elem::LOADABLE));
741 }
742
~LoadIncForce(void)743 LoadIncForce::~LoadIncForce(void)
744 {
745 NO_OP;
746 }
747
748 void
Output(OutputHandler & OH) const749 LoadIncForce::Output(OutputHandler& OH) const
750 {
751 if (bToBeOutput()) {
752 std::ostream& out = OH.Loadable();
753
754 out << GetLabel() << "@" << m_pDNode->GetLabel();
755 if (!m_bCouple) {
756 out << " " << m_F;
757 }
758 if (m_pNode != 0) {
759 out << " " << m_M;
760 }
761 out << std::endl;
762 }
763 }
764
765 void
AfterPredict(VectorHandler & X,VectorHandler & XP)766 LoadIncForce::AfterPredict(VectorHandler& X, VectorHandler& XP)
767 {
768 NO_OP;
769 }
770
771 void
WorkSpaceDim(integer * piNumRows,integer * piNumCols) const772 LoadIncForce::WorkSpaceDim(integer* piNumRows, integer* piNumCols) const
773 {
774 if (m_bCouple || m_pNode == 0) {
775 *piNumRows = 3;
776
777 } else {
778 *piNumRows = 6;
779 }
780
781 if (m_pNode != 0 && (m_bFollower || !m_bCouple)) {
782 *piNumCols = 3 + 1;
783
784 } else {
785 *piNumCols = 1;
786 }
787 }
788
789 VariableSubMatrixHandler&
AssJac(VariableSubMatrixHandler & WorkMat,doublereal dCoef,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)790 LoadIncForce::AssJac(VariableSubMatrixHandler& WorkMat,
791 doublereal dCoef,
792 const VectorHandler& XCurr,
793 const VectorHandler& XPrimeCurr)
794 {
795 // if m_pLoadIncNorm is driven, make sure the LoadIncForce
796 // and the LoadIncNorm are simultaneously active
797 if (m_pDrivenLoadIncNorm && !m_pDrivenLoadIncNorm->bIsActive()) {
798 silent_cerr("LoadIncForce(" << GetLabel() << "): LoadIncNorm(" << m_pLoadIncNorm->GetLabel() << ") inactive" << std::endl);
799 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
800 }
801
802 FullSubMatrixHandler& WM = WorkMat.SetFull();
803
804 integer iNumRows, iNumCols;
805 WorkSpaceDim(&iNumRows, &iNumCols);
806 WM.ResizeReset(iNumRows, iNumCols);
807
808 // std::cerr << "### " << __PRETTY_FUNCTION__ << " iNumRows=" << iNumRows << " iNumCols=" << iNumCols << std::endl;
809
810 if (m_bCouple) {
811 ASSERT(m_pNode != 0);
812 integer iFirstPositionIndex = m_pNode->iGetFirstPositionIndex() + 3;
813 integer iFirstMomentumIndex = m_pNode->iGetFirstMomentumIndex() + 3;
814 integer iNormIndex = m_pLoadIncNorm->iGetFirstIndex() + 1;
815
816 for (integer iCnt = 1; iCnt <= 3; iCnt++) {
817 WM.PutRowIndex(iCnt, iFirstMomentumIndex + iCnt);
818 }
819
820 if (m_bFollower) {
821 for (integer iCnt = 1; iCnt <= 3; iCnt++) {
822 WM.PutColIndex(iCnt, iFirstPositionIndex + iCnt);
823 }
824 WM.PutColIndex(4, iNormIndex);
825
826 WM.Add(1, 1, Mat3x3(MatCross, m_M*dCoef));
827 WM.Sub(1, 4, m_pNode->GetRRef()*m_Dir);
828
829 } else {
830 WM.PutColIndex(1, iNormIndex);
831 WM.Sub(1, 1, m_Dir);
832 }
833
834 } else if (m_pNode == 0) {
835 // structural displacement node
836 integer iFirstMomentumIndex = m_pDNode->iGetFirstMomentumIndex();
837 integer iNormIndex = m_pLoadIncNorm->iGetFirstIndex() + 1;
838
839 WM.PutColIndex(1, iNormIndex);
840
841 for (integer iCnt = 1; iCnt <= 3; iCnt++) {
842 WM.PutRowIndex(iCnt, iFirstMomentumIndex + iCnt);
843 }
844
845 WM.Sub(1, 1, m_Dir);
846
847 } else {
848 integer iFirstPositionIndex = m_pNode->iGetFirstPositionIndex() + 3;
849 integer iFirstMomentumIndex = m_pNode->iGetFirstMomentumIndex();
850 integer iNormIndex = m_pLoadIncNorm->iGetFirstIndex() + 1;
851
852 for (integer iCnt = 1; iCnt <= 3; iCnt++) {
853 WM.PutColIndex(iCnt, iFirstPositionIndex + iCnt);
854 }
855 WM.PutColIndex(4, iNormIndex);
856
857 for (integer iCnt = 1; iCnt <= 6; iCnt++) {
858 WM.PutRowIndex(iCnt, iFirstMomentumIndex + iCnt);
859 }
860
861 const Mat3x3& R(m_pNode->GetRRef());
862
863 if (m_bFollower) {
864 WM.Add(1, 1, Mat3x3(MatCross, m_F*dCoef));
865 WM.Add(3 + 1, 1, Mat3x3(MatCross, m_M*dCoef));
866
867 WM.Sub(1, 4, R*m_Dir);
868 WM.Sub(3 + 1, 4, R*m_o.Cross(m_Dir));
869
870 } else {
871 Vec3 TmpArm(R*m_o);
872
873 WM.Sub(3 + 1, 1, Mat3x3(MatCrossCross, m_F, TmpArm*dCoef));
874
875 WM.Sub(1, 4, m_Dir);
876 WM.Sub(3 + 1, 4, TmpArm.Cross(m_Dir));
877 }
878 }
879
880 return WorkMat;
881 }
882
883 SubVectorHandler&
AssRes(SubVectorHandler & WorkVec,doublereal dCoef,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)884 LoadIncForce::AssRes(SubVectorHandler& WorkVec,
885 doublereal dCoef,
886 const VectorHandler& XCurr,
887 const VectorHandler& XPrimeCurr)
888 {
889 // if m_pLoadIncNorm is driven, make sure the LoadIncForce
890 // and the LoadIncNorm are simultaneously active
891 if (m_pDrivenLoadIncNorm && !m_pDrivenLoadIncNorm->bIsActive()) {
892 silent_cerr("LoadIncForce(" << GetLabel() << "): LoadIncNorm(" << m_pLoadIncNorm->GetLabel() << ") inactive" << std::endl);
893 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
894 }
895
896 integer iNumRows, iNumCols;
897 WorkSpaceDim(&iNumRows, &iNumCols);
898 WorkVec.ResizeReset(iNumRows);
899
900 doublereal dp = m_pLoadIncNorm->dGetP();
901
902 if (m_bCouple) {
903 ASSERT(m_pNode != 0);
904 integer iFirstMomentumIndex = m_pNode->iGetFirstMomentumIndex() + 3;
905 for (integer iCnt = 1; iCnt <= 3; iCnt++) {
906 WorkVec.PutRowIndex(iCnt, iFirstMomentumIndex + iCnt);
907 }
908
909 if (m_bFollower) {
910 const Mat3x3& R(m_pNode->GetRCurr());
911 m_M = R*(m_Dir*dp);
912
913 } else {
914 m_M = m_Dir*dp;
915 }
916
917 WorkVec.Add(1, m_M);
918
919 } else if (m_pNode == 0) {
920 // structural displacement node
921 integer iFirstMomentumIndex = m_pDNode->iGetFirstMomentumIndex();
922 for (integer iCnt = 1; iCnt <= 3; iCnt++) {
923 WorkVec.PutRowIndex(iCnt, iFirstMomentumIndex + iCnt);
924 }
925
926 m_F = m_Dir*dp;
927 WorkVec.Add(1, m_F);
928
929 } else {
930 integer iFirstMomentumIndex = m_pNode->iGetFirstMomentumIndex();
931 for (integer iCnt = 1; iCnt <= 6; iCnt++) {
932 WorkVec.PutRowIndex(iCnt, iFirstMomentumIndex + iCnt);
933 }
934
935 const Mat3x3& R(m_pNode->GetRCurr());
936
937 if (m_bFollower) {
938 Vec3 FTmp(m_Dir*dp);
939
940 m_F = R*FTmp;
941 m_M = R*m_o.Cross(FTmp);
942
943 } else {
944 m_F = m_Dir*dp;
945 m_M = (R*m_o).Cross(m_F);
946 }
947
948 WorkVec.Add(1, m_F);
949 WorkVec.Add(4, m_M);
950 }
951
952 return WorkVec;
953 }
954
955 unsigned int
iGetNumPrivData(void) const956 LoadIncForce::iGetNumPrivData(void) const
957 {
958 return 0;
959 }
960
961 int
iGetNumConnectedNodes(void) const962 LoadIncForce::iGetNumConnectedNodes(void) const
963 {
964 return 1;
965 }
966
967 void
GetConnectedNodes(std::vector<const Node * > & connectedNodes) const968 LoadIncForce::GetConnectedNodes(std::vector<const Node *>& connectedNodes) const
969 {
970 connectedNodes.resize(1);
971
972 connectedNodes[0] = m_pDNode;
973 }
974
975 void
SetValue(DataManager * pDM,VectorHandler & X,VectorHandler & XP,SimulationEntity::Hints * ph)976 LoadIncForce::SetValue(DataManager *pDM,
977 VectorHandler& X, VectorHandler& XP,
978 SimulationEntity::Hints *ph)
979 {
980 NO_OP;
981 }
982
983 std::ostream&
Restart(std::ostream & out) const984 LoadIncForce::Restart(std::ostream& out) const
985 {
986 return out << "# LoadIncForce: not implemented" << std::endl;
987 }
988
989 unsigned int
iGetInitialNumDof(void) const990 LoadIncForce::iGetInitialNumDof(void) const
991 {
992 return 0;
993 }
994
995 void
InitialWorkSpaceDim(integer * piNumRows,integer * piNumCols) const996 LoadIncForce::InitialWorkSpaceDim(
997 integer* piNumRows,
998 integer* piNumCols) const
999 {
1000 *piNumRows = 0;
1001 *piNumCols = 0;
1002 }
1003
1004 VariableSubMatrixHandler&
InitialAssJac(VariableSubMatrixHandler & WorkMat,const VectorHandler & XCurr)1005 LoadIncForce::InitialAssJac(
1006 VariableSubMatrixHandler& WorkMat,
1007 const VectorHandler& XCurr)
1008 {
1009 // should not be called, since initial workspace is empty
1010 ASSERT(0);
1011
1012 WorkMat.SetNullMatrix();
1013
1014 return WorkMat;
1015 }
1016
1017 SubVectorHandler&
InitialAssRes(SubVectorHandler & WorkVec,const VectorHandler & XCurr)1018 LoadIncForce::InitialAssRes(
1019 SubVectorHandler& WorkVec,
1020 const VectorHandler& XCurr)
1021 {
1022 // should not be called, since initial workspace is empty
1023 ASSERT(0);
1024
1025 WorkVec.ResizeReset(0);
1026
1027 return WorkVec;
1028 }
1029
1030 extern "C" int
module_init(const char * module_name,void * pdm,void * php)1031 module_init(const char *module_name, void *pdm, void *php)
1032 {
1033 UserDefinedElemRead *rf;
1034
1035 rf = new UDERead<LoadIncForce>;
1036 if (!SetUDE("load" "increment" "force", rf)) {
1037 delete rf;
1038
1039 silent_cerr("module-loadinc: "
1040 "module_init(" << module_name << ") "
1041 "failed" << std::endl);
1042
1043 return -1;
1044 }
1045
1046 rf = new UDERead<LoadIncNorm>;
1047 if (!SetUDE("load" "increment" "normalization", rf)) {
1048 delete rf;
1049
1050 silent_cerr("module-loadinc: "
1051 "module_init(" << module_name << ") "
1052 "failed" << std::endl);
1053
1054 return -1;
1055 }
1056
1057 return 0;
1058 }
1059
1060