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