1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/distance.cc,v 1.47 2017/05/12 17:29:26 morandini 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 /* Vincoli generali */
33
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35
36 #include <limits>
37
38 #ifdef MBDYN_X_DISTANCE_JOINT
39
40 #include "distance.h"
41 #include "hint.h"
42 #include "hint_impl.h"
43
44 /* DistanceJoint - begin */
45
46 /* Costruttore non banale */
DistanceJoint(unsigned int uL,const DofOwner * pDO,const StructDispNode * pN1,const StructDispNode * pN2,const DriveCaller * pDC,flag fOut)47 DistanceJoint::DistanceJoint(unsigned int uL, const DofOwner* pDO,
48 const StructDispNode* pN1, const StructDispNode* pN2,
49 const DriveCaller* pDC, flag fOut)
50 : Elem(uL, fOut),
51 Joint(uL, pDO, fOut),
52 DriveOwner(pDC),
53 pNode1(pN1), pNode2(pN2), Vec(Zero3), dAlpha(0.)
54 {
55 NO_OP;
56 }
57
58 /* Distruttore banale - ci pensa il DriveOwner a distruggere il DriveCaller */
~DistanceJoint(void)59 DistanceJoint::~DistanceJoint(void)
60 {
61 NO_OP;
62 }
63
64 void
Abort(void)65 DistanceJoint::Abort(void)
66 {
67 silent_cerr("DistanceJoint(" << GetLabel() << "): distance is null"
68 << std::endl);
69 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
70 }
71
72 void
SetValue(DataManager * pDM,VectorHandler & X,VectorHandler &,SimulationEntity::Hints * ph)73 DistanceJoint::SetValue(DataManager *pDM,
74 VectorHandler& X, VectorHandler& /* XP */ ,
75 SimulationEntity::Hints *ph)
76 {
77 if (ph) {
78 for (unsigned i = 0; i < ph->size(); i++) {
79 DriveHint *pdh = dynamic_cast<DriveHint *>((*ph)[i]);
80
81 if (pdh) {
82 pedantic_cout("DistanceJoint(" << uLabel << "): "
83 "creating drive from hint[" << i << "]..." << std::endl);
84
85 DriveCaller *pDC = pdh->pCreateDrive(pDM);
86 if (pDC == 0) {
87 silent_cerr("DistanceJoint(" << uLabel << "): "
88 "unable to create drive after hint "
89 "#" << i << std::endl);
90 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
91 }
92
93 DriveOwner::Set(pDC);
94 continue;
95 }
96 }
97 }
98
99 doublereal dDistance = pGetDriveCaller()->dGet();
100
101 /* Setta a 1 dAlpha, che e' indipendente dalle altre variabili
102 * in caso di distanza nulla */
103 if (fabs(dDistance) <= std::numeric_limits<doublereal>::epsilon()) {
104 Abort();
105 }
106
107 /* Scrive la direzione della distanza. Se e' stata ottenuta con
108 * l'assemblaggio iniziale bene, se no' la calcola */
109 Vec = pNode2->GetXCurr() - pNode1->GetXCurr();
110 doublereal d = Vec.Dot();
111 if (d <= std::numeric_limits<doublereal>::epsilon()) {
112 silent_cerr("DistanceJoint(" << uLabel << ") "
113 "linked to nodes " << pNode1->GetLabel()
114 << " and " << pNode2->GetLabel() << ": "
115 "nodes are coincident;" << std::endl
116 << "initial joint assembly is recommended"
117 << std::endl);
118 throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
119 }
120 }
121
122 /* Dati privati */
123 unsigned int
iGetNumPrivData(void) const124 DistanceJoint::iGetNumPrivData(void) const
125 {
126 return 1;
127 }
128
129 unsigned int
iGetPrivDataIdx(const char * s) const130 DistanceJoint::iGetPrivDataIdx(const char *s) const
131 {
132 ASSERT(s != NULL);
133
134 if (strcmp(s, "d") == 0) {
135 return 1;
136 }
137
138 return 0;
139 }
140
141 doublereal
dGetPrivData(unsigned int i) const142 DistanceJoint::dGetPrivData(unsigned int i) const
143 {
144 ASSERT(i == 1);
145
146 if (i == 1) {
147 #if 0
148 /* FIXME: could simply use */
149 return dDistance;
150 #endif
151 return dGet();
152 }
153
154 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
155 }
156
157 /* Contributo al file di restart */
Restart(std::ostream & out) const158 std::ostream& DistanceJoint::Restart(std::ostream& out) const
159 {
160 Joint::Restart(out) << ", distance, "
161 << pNode1->GetLabel() << ", "
162 << pNode2->GetLabel() << ", ",
163 pGetDriveCaller()->Restart(out) << ';' << std::endl;
164 return out;
165 }
166
167
168 /* Assemblaggio matrici */
169 void
AssMat(FullSubMatrixHandler & WorkMatA,FullSubMatrixHandler & WorkMatB,doublereal dCoef,const VectorHandler & XCurr,const VectorHandler &)170 DistanceJoint::AssMat(FullSubMatrixHandler& WorkMatA,
171 FullSubMatrixHandler& WorkMatB,
172 doublereal dCoef,
173 const VectorHandler& XCurr,
174 const VectorHandler& /* XPrimeCurr */ )
175 {
176 DEBUGCOUT("Entering DistanceJoint::AssMat()" << std::endl);
177
178 doublereal dd = dAlpha*dCoef;
179 doublereal dv = Vec.Norm();
180 for (int iCnt = 1; iCnt <= 3; iCnt++) {
181 doublereal d = Vec.dGet(iCnt);
182
183 /* Constraint equation */
184
185 /* C: - U^T/|U| Delta x_1 */
186 WorkMatB.DecCoef(6 + 1, 0 + iCnt, d/dv);
187
188 /* C: U^T/|U| Delta x_2 */
189 WorkMatB.IncCoef(6 + 1, 3 + iCnt, d/dv);
190
191 /* Equilibrium */
192
193 /* F1: - U Delta lambda */
194 WorkMatB.DecCoef(0 + iCnt, 6 + 1, d);
195
196 /* F2: U Delta lambda */
197 WorkMatB.IncCoef(3 + iCnt, 6 + 1, d);
198
199 /* F1: lambda Delta x_1 */
200 WorkMatA.IncCoef(0 + iCnt, 0 + iCnt, d*dd);
201
202 /* F1: - lambda Delta x_2 */
203 WorkMatA.DecCoef(0 + iCnt, 3 + iCnt, d*dd);
204
205 /* F2: - lambda Delta x_1 */
206 WorkMatA.DecCoef(3 + iCnt, 0 + iCnt, d*dd);
207
208 /* F2: lambda Delta x_2 */
209 WorkMatA.IncCoef(3 + iCnt, 3 + iCnt, d*dd);
210 }
211 }
212
213
214 /* Assemblaggio jacobiano */
215 VariableSubMatrixHandler&
AssJac(VariableSubMatrixHandler & WorkMat,doublereal dCoef,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)216 DistanceJoint::AssJac(VariableSubMatrixHandler& WorkMat,
217 doublereal dCoef,
218 const VectorHandler& XCurr,
219 const VectorHandler& XPrimeCurr)
220 {
221 DEBUGCOUT("Entering DistanceJoint::AssJac()" << std::endl);
222
223 FullSubMatrixHandler& WM = WorkMat.SetFull();
224 WM.ResizeReset(7, 7);
225
226 integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
227 integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
228 integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
229 integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
230 integer iFirstReactionIndex = iGetFirstIndex();
231
232 for (int iCnt = 1; iCnt <= 3; iCnt++) {
233 WM.PutRowIndex(0 + iCnt, iNode1FirstMomIndex + iCnt);
234 WM.PutColIndex(0 + iCnt, iNode1FirstPosIndex + iCnt);
235 WM.PutRowIndex(3 + iCnt, iNode2FirstMomIndex + iCnt);
236 WM.PutColIndex(3 + iCnt, iNode2FirstPosIndex + iCnt);
237 }
238 WM.PutRowIndex(6 + 1, iFirstReactionIndex + 1);
239 WM.PutColIndex(6 + 1, iFirstReactionIndex + 1);
240
241 AssMat(WM, WM, dCoef, XCurr, XPrimeCurr);
242
243 return WorkMat;
244 }
245
246
247 /* Assemblaggio jacobiano */
248 void
AssMats(VariableSubMatrixHandler & WorkMatA,VariableSubMatrixHandler & WorkMatB,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)249 DistanceJoint::AssMats(VariableSubMatrixHandler& WorkMatA,
250 VariableSubMatrixHandler& WorkMatB,
251 const VectorHandler& XCurr,
252 const VectorHandler& XPrimeCurr)
253 {
254 DEBUGCOUT("Entering DistanceJoint::AssMats()" << std::endl);
255
256 FullSubMatrixHandler& WMA = WorkMatA.SetFull();
257 WMA.ResizeReset(7, 7);
258
259 FullSubMatrixHandler& WMB = WorkMatB.SetFull();
260 WMB.ResizeReset(7, 7);
261
262 integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
263 integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
264 integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
265 integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
266 integer iFirstReactionIndex = iGetFirstIndex();
267
268 for (int iCnt = 1; iCnt <= 3; iCnt++) {
269 WMA.PutRowIndex(0 + iCnt, iNode1FirstMomIndex + iCnt);
270 WMA.PutColIndex(0 + iCnt, iNode1FirstPosIndex + iCnt);
271 WMA.PutRowIndex(3 + iCnt, iNode2FirstMomIndex + iCnt);
272 WMA.PutColIndex(3 + iCnt, iNode2FirstPosIndex + iCnt);
273
274 WMB.PutRowIndex(0 + iCnt, iNode1FirstMomIndex + iCnt);
275 WMB.PutColIndex(0 + iCnt, iNode1FirstPosIndex + iCnt);
276 WMB.PutRowIndex(3 + iCnt, iNode2FirstMomIndex + iCnt);
277 WMB.PutColIndex(3 + iCnt, iNode2FirstPosIndex + iCnt);
278 }
279 WMA.PutRowIndex(6 + 1, iFirstReactionIndex + 1);
280 WMA.PutColIndex(6 + 1, iFirstReactionIndex + 1);
281
282 WMB.PutRowIndex(6 + 1, iFirstReactionIndex + 1);
283 WMB.PutColIndex(6 + 1, iFirstReactionIndex + 1);
284
285 AssMat(WMA, WMB, 1., XCurr, XPrimeCurr);
286 }
287
288
289 /* Assemblaggio residuo */
290 SubVectorHandler&
AssRes(SubVectorHandler & WorkVec,doublereal dCoef,const VectorHandler & XCurr,const VectorHandler &)291 DistanceJoint::AssRes(SubVectorHandler& WorkVec,
292 doublereal dCoef,
293 const VectorHandler& XCurr,
294 const VectorHandler& /* XPrimeCurr */ )
295 {
296 DEBUGCOUT("Entering DistanceJoint::AssRes()" << std::endl);
297
298 /* Dimensiona e resetta la matrice di lavoro */
299 integer iNumRows = 0;
300 integer iNumCols = 0;
301 WorkSpaceDim(&iNumRows, &iNumCols);
302 WorkVec.ResizeReset(iNumRows);
303
304 integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
305 integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
306 integer iFirstReactionIndex = iGetFirstIndex();
307
308 for (int iCnt = 1; iCnt <= 3; iCnt++) {
309 /* Indici del nodo 1 */
310 WorkVec.PutRowIndex(0 + iCnt, iNode1FirstMomIndex + iCnt);
311
312 /* Indici del nodo 2 */
313 WorkVec.PutRowIndex(3 + iCnt, iNode2FirstMomIndex + iCnt);
314 }
315
316 /* Indici del vincolo */
317 WorkVec.PutRowIndex(6 + 1, iFirstReactionIndex + 1);
318
319 Vec = pNode2->GetXCurr() - pNode1->GetXCurr();
320
321 /* Aggiorna i dati propri */
322 dAlpha = XCurr(iFirstReactionIndex + 1);
323
324 dDistance = pGetDriveCaller()->dGet();
325
326 /* Distanza nulla */
327 if (fabs(dDistance) <= std::numeric_limits<doublereal>::epsilon()) {
328 Abort();
329 }
330
331 WorkVec.IncCoef(6 + 1, (dDistance - Vec.Norm())/dCoef);
332
333 Vec3 Tmp(Vec*dAlpha);
334 WorkVec.Add(0 + 1, Tmp);
335 WorkVec.Sub(3 + 1, Tmp);
336
337 return WorkVec;
338 }
339
340 void
Output(OutputHandler & OH) const341 DistanceJoint::Output(OutputHandler& OH) const
342 {
343 if (bToBeOutput()) {
344 Joint::Output(OH.Joints(), "Distance", GetLabel(),
345 Vec3(dAlpha, 0., 0.), Zero3, Vec*dAlpha, Zero3)
346 << " " << Vec/dDistance << " " << dDistance
347 << std::endl;
348 }
349 }
350
351 /* Nota: vanno modificati in analogia al DistanceWithOffsetJoint */
352
353 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
354 VariableSubMatrixHandler&
InitialAssJac(VariableSubMatrixHandler & WorkMat,const VectorHandler & XCurr)355 DistanceJoint::InitialAssJac(VariableSubMatrixHandler& WorkMat,
356 const VectorHandler& XCurr)
357 {
358 DEBUGCOUT("Entering DistanceJoint::InitialAssJac()" << std::endl);
359
360 WorkMat.SetNullMatrix();
361
362 return WorkMat;
363 }
364
365
366 /* Contributo al residuo durante l'assemblaggio iniziale */
367 SubVectorHandler&
InitialAssRes(SubVectorHandler & WorkVec,const VectorHandler & XCurr)368 DistanceJoint::InitialAssRes(SubVectorHandler& WorkVec,
369 const VectorHandler& XCurr)
370 {
371 DEBUGCOUT("Entering DistanceJoint::InitialAssRes()" << std::endl);
372
373 /* Dimensiona e resetta la matrice di lavoro */
374 WorkVec.Resize(0);
375
376 return WorkVec;
377 }
378
379
380 /* DistanceJoint - end */
381
382
383 /* DistanceJointWithOffset - begin */
384
385 /* Costruttore non banale */
DistanceJointWithOffset(unsigned int uL,const DofOwner * pDO,const StructNode * pN1,const StructNode * pN2,const Vec3 & f1Tmp,const Vec3 & f2Tmp,const DriveCaller * pDC,flag fOut)386 DistanceJointWithOffset::DistanceJointWithOffset(unsigned int uL,
387 const DofOwner* pDO,
388 const StructNode* pN1,
389 const StructNode* pN2,
390 const Vec3& f1Tmp,
391 const Vec3& f2Tmp,
392 const DriveCaller* pDC,
393 flag fOut)
394 : Elem(uL, fOut),
395 DistanceJoint(uL, pDO, pN1, pN2, pDC, fOut),
396 f1(f1Tmp), f2(f2Tmp)
397 {
398 NO_OP;
399 }
400
401 /* Distruttore banale - ci pensa il DriveOwner a distruggere il DriveCaller */
~DistanceJointWithOffset(void)402 DistanceJointWithOffset::~DistanceJointWithOffset(void)
403 {
404 NO_OP;
405 }
406
407 /* Contributo al file di restart */
408 std::ostream&
Restart(std::ostream & out) const409 DistanceJointWithOffset::Restart(std::ostream& out) const
410 {
411 Joint::Restart(out) << ", distance with offset, "
412 << pNode1->GetLabel()
413 << ", reference, node, ",
414 f1.Write(out, ", ") << ", "
415 << pNode2->GetLabel()
416 << ", reference, node, ",
417 f2.Write(out, ", ") << ", ";
418 return pGetDriveCaller()->Restart(out) << ';' << std::endl;
419 }
420
421 void
SetValue(DataManager * pDM,VectorHandler & X,VectorHandler &,SimulationEntity::Hints * ph)422 DistanceJointWithOffset::SetValue(DataManager *pDM,
423 VectorHandler& X, VectorHandler& /* XP */ ,
424 SimulationEntity::Hints *ph)
425 {
426 if (ph) {
427 for (unsigned i = 0; i < ph->size(); i++) {
428 pedantic_cout("DistanceJointWithOffset(" << uLabel << "): "
429 "creating drive from hint..." << std::endl);
430
431 DriveHint *pdh = dynamic_cast<DriveHint *>((*ph)[i]);
432
433 if (pdh) {
434 DriveCaller *pDC = pdh->pCreateDrive(pDM);
435 if (pDC == 0) {
436 silent_cerr("DistanceJointWithOffset(" << uLabel << "): "
437 "unable to create drive after hint "
438 "#" << i << std::endl);
439 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
440 }
441
442 DriveOwner::Set(pDC);
443 continue;
444 }
445 }
446 }
447
448 doublereal dDistance = pGetDriveCaller()->dGet();
449
450 /* Setta a 1 dAlpha, che e' indipendente dalle altre variabili
451 * in caso di distanza nulla */
452 if (fabs(dDistance) <= std::numeric_limits<doublereal>::epsilon()) {
453 Abort();
454 }
455
456 Vec = pNode2->GetXCurr() + dynamic_cast<const StructNode *>(pNode2)->GetRCurr()*f2
457 - pNode1->GetXCurr() - dynamic_cast<const StructNode *>(pNode1)->GetRCurr()*f1;
458 doublereal d = Vec.Dot();
459 if (d <= std::numeric_limits<doublereal>::epsilon()) {
460 silent_cerr("DistanceJoint(" << GetLabel() << ") "
461 "linked to nodes " << pNode1->GetLabel()
462 << " and " << pNode2->GetLabel() << ": "
463 "nodes are coincident;" << std::endl
464 << "this is no longer supported" << std::endl);
465 throw ErrNullNorm(MBDYN_EXCEPT_ARGS);
466 }
467 }
468
469 /* Assemblaggio matrici */
470 void
AssMat(FullSubMatrixHandler & WorkMatA,FullSubMatrixHandler & WorkMatB,doublereal dCoef,const VectorHandler &,const VectorHandler &)471 DistanceJointWithOffset::AssMat(FullSubMatrixHandler& WorkMatA,
472 FullSubMatrixHandler& WorkMatB,
473 doublereal dCoef,
474 const VectorHandler& /* XCurr */ ,
475 const VectorHandler& /* XPrimeCurr */ )
476 {
477 DEBUGCOUT("Entering DistanceJointWithOffset::AssMat()" << std::endl);
478
479 Vec3 f1Tmp(dynamic_cast<const StructNode *>(pNode1)->GetRRef()*f1);
480 Vec3 f2Tmp(dynamic_cast<const StructNode *>(pNode2)->GetRRef()*f2);
481
482 Vec3 f1u(f1Tmp.Cross(Vec));
483 Vec3 f2u(f2Tmp.Cross(Vec));
484
485 doublereal dd = dAlpha*dCoef;
486 doublereal dv = Vec.Norm();
487 for (int iCnt = 1; iCnt <= 3; iCnt++) {
488 doublereal d = Vec.dGet(iCnt);
489
490 /* Constraint equation */
491
492 /* C: - U^T/|U| Delta x_1 */
493 WorkMatB.DecCoef(12 + 1, 0 + iCnt, d/dv);
494
495 /* C: U^T/|U| Delta x_2 */
496 WorkMatB.IncCoef(12 + 1, 6 + iCnt, d/dv);
497
498 /* Equilibrium */
499
500 /* F1: - U Delta lambda */
501 WorkMatB.DecCoef(0 + iCnt, 12 + 1, d);
502
503 /* F2: U Delta lambda */
504 WorkMatB.IncCoef(6 + iCnt, 12 + 1, d);
505
506 /* F1: lambda Delta x_1 */
507 WorkMatA.IncCoef(0 + iCnt, 0 + iCnt, dd);
508
509 /* F1: - lambda Delta x_2 */
510 WorkMatA.DecCoef(0 + iCnt, 6 + iCnt, dd);
511
512 /* F2: - lambda Delta x_1 */
513 WorkMatA.DecCoef(6 + iCnt, 0 + iCnt, dd);
514
515 /* F2: lambda Delta x_2 */
516 WorkMatA.IncCoef(6 + iCnt, 6 + iCnt, dd);
517
518 d = f1u.dGet(iCnt);
519
520 /* C: - (f1 Cross U)^T/|U| Delta g_1 */
521 WorkMatB.DecCoef(12 + 1, 3 + iCnt, d/dv);
522
523 /* M1: - (f1 Cross U) Delta lambda */
524 WorkMatB.DecCoef(3 + iCnt, 12 + 1, d);
525
526 d = f2u.dGet(iCnt);
527
528 /* C: (f2 Cross U)^T/|U| Delta g_2 */
529 WorkMatB.IncCoef(12 + 1, 9 + iCnt, d/dv);
530
531 /* M2: (f2 Cross U) Delta lambda */
532 WorkMatB.IncCoef(9 + iCnt, 12 + 1, d);
533 }
534
535 Mat3x3 Tmp;
536
537 Tmp = Mat3x3(MatCross, f1Tmp*dd);
538
539 /* F1: - lambda f1 Cross Delta g_1 */
540 WorkMatA.Sub(0 + 1, 3 + 1, Tmp);
541
542 /* F2: lambda f1 Cross Delta g_1 */
543 WorkMatA.Add(6 + 1, 3 + 1, Tmp);
544
545 /* M1: lambda f1 Cross Delta x_1 */
546 WorkMatA.Add(3 + 1, 0 + 1, Tmp);
547
548 /* M1: - lambda f1 Cross Delta x_2 */
549 WorkMatA.Sub(3 + 1, 6 + 1, Tmp);
550
551 Tmp = Mat3x3(MatCross, f2Tmp*dd);
552
553 /* F1: lambda f2 Cross Delta g_2 */
554 WorkMatA.Add(0 + 1, 9 + 1, Tmp);
555
556 /* F2: - lambda f2 Cross Delta g_2 */
557 WorkMatA.Sub(6 + 1, 9 + 1, Tmp);
558
559 /* M2: - lambda f2 Cross Delta x_1 */
560 WorkMatA.Sub(9 + 1, 0 + 1, Tmp);
561
562 /* M2: lambda f2 Cross Delta x_2 */
563 WorkMatA.Add(9 + 1, 6 + 1, Tmp);
564
565 Tmp = Mat3x3(MatCrossCross, f1Tmp, f2Tmp*dd);
566
567 /* M1: lambda f1 Cross f2 Cross Delta g_2 */
568 WorkMatA.Add(3 + 1, 9 + 1, Tmp);
569
570 Tmp = Mat3x3(MatCrossCross, f2Tmp, f1Tmp*dd);
571
572 /* M2: lambda f2 Cross f1 Cross Delta g_1 */
573 WorkMatA.Add(9 + 1, 3 + 1, Tmp);
574
575 Tmp = Mat3x3(MatCrossCross, Vec + f1Tmp, f1Tmp*dd);
576
577 /* M1: - lambda (x2 + f2 - x1) Cross f1 Cross Delta g_1 */
578 WorkMatA.Sub(3 + 1, 3 + 1, Tmp);
579
580 Tmp = Mat3x3(MatCrossCross, Vec - f2Tmp, f2Tmp*dd);
581
582 /* M2: - lambda (x2 - x1 - f1) Cross f2 Cross Delta g_2 */
583 WorkMatA.Add(9 + 1, 9 + 1, Tmp);
584 }
585
586 /* Assemblaggio jacobiano */
587 VariableSubMatrixHandler&
AssJac(VariableSubMatrixHandler & WorkMat,doublereal dCoef,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)588 DistanceJointWithOffset::AssJac(VariableSubMatrixHandler& WorkMat,
589 doublereal dCoef,
590 const VectorHandler& XCurr,
591 const VectorHandler& XPrimeCurr)
592 {
593 DEBUGCOUT("Entering DistanceJointWithOffset::AssJac()" << std::endl);
594
595 FullSubMatrixHandler& WM = WorkMat.SetFull();
596 WM.ResizeReset(13, 13);
597
598 integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
599 integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
600 integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
601 integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
602 integer iFirstReactionIndex = iGetFirstIndex();
603
604 for (int iCnt = 1; iCnt <= 6; iCnt++) {
605 WM.PutRowIndex(0 + iCnt, iNode1FirstMomIndex + iCnt);
606 WM.PutColIndex(0 + iCnt, iNode1FirstPosIndex + iCnt);
607 WM.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
608 WM.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
609 }
610 WM.PutRowIndex(12 + 1, iFirstReactionIndex + 1);
611 WM.PutColIndex(12 + 1, iFirstReactionIndex + 1);
612
613 AssMat(WM, WM, dCoef, XCurr, XPrimeCurr);
614
615 return WorkMat;
616 }
617
618 /* Assemblaggio matrici */
619 void
AssMats(VariableSubMatrixHandler & WorkMatA,VariableSubMatrixHandler & WorkMatB,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)620 DistanceJointWithOffset::AssMats(VariableSubMatrixHandler& WorkMatA,
621 VariableSubMatrixHandler& WorkMatB,
622 const VectorHandler& XCurr,
623 const VectorHandler& XPrimeCurr)
624 {
625 DEBUGCOUT("Entering DistanceJointWithOffset::AssMats()" << std::endl);
626
627 FullSubMatrixHandler& WMA = WorkMatA.SetFull();
628 WMA.ResizeReset(13, 13);
629
630 FullSubMatrixHandler& WMB = WorkMatB.SetFull();
631 WMB.ResizeReset(13, 13);
632
633 integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
634 integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
635 integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
636 integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
637 integer iFirstReactionIndex = iGetFirstIndex();
638
639 for (int iCnt = 1; iCnt <= 6; iCnt++) {
640 WMA.PutRowIndex(0 + iCnt, iNode1FirstMomIndex + iCnt);
641 WMA.PutColIndex(0 + iCnt, iNode1FirstPosIndex + iCnt);
642 WMA.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
643 WMA.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
644
645 WMB.PutRowIndex(0 + iCnt, iNode1FirstMomIndex + iCnt);
646 WMB.PutColIndex(0 + iCnt, iNode1FirstPosIndex + iCnt);
647 WMB.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
648 WMB.PutColIndex(6 + iCnt, iNode2FirstPosIndex + iCnt);
649 }
650 WMA.PutRowIndex(12 + 1, iFirstReactionIndex + 1);
651 WMA.PutColIndex(12 + 1, iFirstReactionIndex + 1);
652
653 WMB.PutRowIndex(12 + 1, iFirstReactionIndex + 1);
654 WMB.PutColIndex(12 + 1, iFirstReactionIndex + 1);
655
656 AssMat(WMA, WMB, 1., XCurr, XPrimeCurr);
657 }
658
659 /* Assemblaggio residuo */
660 SubVectorHandler&
AssRes(SubVectorHandler & WorkVec,doublereal dCoef,const VectorHandler & XCurr,const VectorHandler &)661 DistanceJointWithOffset::AssRes(SubVectorHandler& WorkVec,
662 doublereal dCoef,
663 const VectorHandler& XCurr,
664 const VectorHandler& /* XPrimeCurr */ )
665 {
666 DEBUGCOUT("Entering DistanceJointWithOffset::AssRes()" << std::endl);
667
668 /* Dimensiona e resetta la matrice di lavoro */
669 integer iNumRows = 0;
670 integer iNumCols = 0;
671 WorkSpaceDim(&iNumRows, &iNumCols);
672 WorkVec.ResizeReset(iNumRows);
673
674 integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
675 integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
676 integer iFirstReactionIndex = iGetFirstIndex();
677
678 for (int iCnt = 1; iCnt <= 6; iCnt++) {
679 /* Indici del nodo 1 */
680 WorkVec.PutRowIndex(0 + iCnt, iNode1FirstMomIndex + iCnt);
681
682 /* Indici del nodo 2 */
683 WorkVec.PutRowIndex(6 + iCnt, iNode2FirstMomIndex + iCnt);
684 }
685
686 /* Indici del vincolo */
687 WorkVec.PutRowIndex(12 + 1, iFirstReactionIndex + 1);
688
689 Vec3 f1Tmp(dynamic_cast<const StructNode *>(pNode1)->GetRCurr()*f1);
690 Vec3 f2Tmp(dynamic_cast<const StructNode *>(pNode2)->GetRCurr()*f2);
691
692 /* x2 + f2 - x1 - f1 */
693 Vec = pNode2->GetXCurr() + f2Tmp
694 - pNode1->GetXCurr() - f1Tmp;
695
696 /* Aggiorna i dati propri */
697 dAlpha = XCurr(iFirstReactionIndex + 1);
698
699 dDistance = pGetDriveCaller()->dGet();
700
701 /* Distanza nulla */
702 if (fabs(dDistance) <= std::numeric_limits<doublereal>::epsilon()) {
703 Abort();
704 }
705
706 WorkVec.IncCoef(12 + 1, (dDistance - Vec.Norm())/dCoef);
707
708 Vec3 Tmp(Vec*dAlpha);
709 WorkVec.Add(0 + 1, Tmp);
710 WorkVec.Add(3 + 1, f1Tmp.Cross(Tmp));
711 WorkVec.Sub(6 + 1, Tmp);
712 WorkVec.Sub(9 + 1, f2Tmp.Cross(Tmp));
713
714 return WorkVec;
715 }
716
717
718 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
719 VariableSubMatrixHandler&
InitialAssJac(VariableSubMatrixHandler & WorkMat,const VectorHandler & XCurr)720 DistanceJointWithOffset::InitialAssJac(VariableSubMatrixHandler& WorkMat,
721 const VectorHandler& XCurr)
722 {
723 DEBUGCOUT("Entering DistanceJointWithOffset::InitialAssJac()"
724 << std::endl);
725
726 WorkMat.SetNullMatrix();
727
728 return WorkMat;
729 }
730
731
732 /* Contributo al residuo durante l'assemblaggio iniziale */
733 SubVectorHandler&
InitialAssRes(SubVectorHandler & WorkVec,const VectorHandler & XCurr)734 DistanceJointWithOffset::InitialAssRes(SubVectorHandler& WorkVec,
735 const VectorHandler& XCurr)
736 {
737 DEBUGCOUT("Entering DistanceJointWithOffset::InitialAssRes()"
738 << std::endl);
739
740 WorkVec.Resize(0);
741
742 return WorkVec;
743 }
744
745 #endif
746
747 /* DistanceJointWithOffset - end */
748