MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
genj.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/genj.cc,v 1.86 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 #include "dataman.h"
39 #include "genj.h"
40 #include "hint.h"
41 #include "hint_impl.h"
42 #include "Rot.hh"
43 
44 #ifndef MBDYN_X_DISTANCE_JOINT
45 
46 /* DistanceJoint - begin */
47 
48 /* Costruttore non banale */
49 DistanceJoint::DistanceJoint(unsigned int uL, const DofOwner* pDO,
50  const StructDispNode* pN1, const StructDispNode* pN2,
51  const DriveCaller* pDC, flag fOut)
52 : Elem(uL, fOut),
53 Joint(uL, pDO, fOut),
54 DriveOwner(pDC),
55 pNode1(pN1), pNode2(pN2), v(Zero3), dAlpha(0.)
56 {
57  NO_OP;
58 }
59 
60 
61 /* Distruttore banale - ci pensa il DriveOwner a distruggere il DriveCaller */
63 {
64  NO_OP;
65 }
66 
67 
68 /* Dati privati */
69 unsigned int
71 {
72  return 1;
73 }
74 
75 unsigned int
76 DistanceJoint::iGetPrivDataIdx(const char *s) const
77 {
78  ASSERT(s != NULL);
79 
80  if (strcmp(s, "d") == 0) {
81  return 1;
82  }
83 
84  return 0;
85 }
86 
88 DistanceJoint::dGetPrivData(unsigned int i) const
89 {
90  ASSERT(i == 1);
91 
92  if (i == 1) {
93  return dGet();
94  }
95 
97 }
98 
99 /* Contributo al file di restart */
100 std::ostream& DistanceJoint::Restart(std::ostream& out) const
101 {
102  Joint::Restart(out) << ", distance, "
103  << pNode1->GetLabel() << ", "
104  << pNode2->GetLabel() << ", ";
105  return pGetDriveCaller()->Restart(out) << ';' << std::endl;
106 }
107 
108 
109 /* Assemblaggio jacobiano */
112  doublereal dCoef,
113  const VectorHandler& /* XCurr */ ,
114  const VectorHandler& /* XPrimeCurr */ )
115 {
116  DEBUGCOUT("Entering DistanceJoint::AssJac()" << std::endl);
117 
118  SparseSubMatrixHandler& WM = WorkMat.SetSparse();
119 
120  /* Dimensiona e resetta la matrice di lavoro */
121  doublereal dDistance = pGetDriveCaller()->dGet();
122 
123  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
124  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
125  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
126  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
127  integer iFirstReactionIndex = iGetFirstIndex();
128 
129  /*
130  * forza: dAlpha*v
131  * distanza: x2 - x1 = d*v
132  * equazione di normalizzazione di v:
133  * (v^T*v)^1/2 = 1
134  *
135  *
136  * x1 Q1 x2 Q2 v a
137  *
138  * x1 | 0 0 0 0 0 0 |
139  * Q1 | 0 0 0 0 -a*I -v |
140  * x2 | 0 0 0 0 0 0 |
141  * Q2 | 0 0 0 0 a*I v |
142  * v | -c*I 0 c*I 0 -d*I 0 |
143  * a | 0 0 0 0 v^T 0 |
144  * ----------
145  * (vT*v)^1/2
146  *
147  * con c = dCoef, a = dAlpha
148  */
149 
150  WM.ResizeReset(24, 0);
151 
152  /* Nota: la direzione della reazione, v,
153  * ed il coefficiente di amplificazione dAlpha
154  * sono stati aggiornati durante il calcolo del residuo */
155 
156  for (int iCnt = 1; iCnt <= 3; iCnt++) {
157  /* termini di Delta_x1 */
158  WM.PutItem(iCnt, iFirstReactionIndex+iCnt,
159  iNode1FirstPosIndex+iCnt, -dCoef);
160 
161  /* termini di Delta_x2 */
162  WM.PutItem(3+iCnt, iFirstReactionIndex+iCnt,
163  iNode2FirstPosIndex+iCnt, dCoef);
164 
165  /* termini di Delta_F sul nodo 1 */
166  WM.PutItem(6+iCnt, iNode1FirstMomIndex+iCnt,
167  iFirstReactionIndex+iCnt, -dAlpha);
168 
169  /* termini di Delta_F sul nodo 2 */
170  WM.PutItem(9+iCnt, iNode2FirstMomIndex+iCnt,
171  iFirstReactionIndex+iCnt, dAlpha);
172 
173  /* termini diagonali di Delta_v */
174  WM.PutItem(12+iCnt, iFirstReactionIndex+iCnt,
175  iFirstReactionIndex+iCnt, -dDistance);
176 
177  /* termini di Delta_alpha sul nodo 1 */
178  WM.PutItem(15+iCnt, iNode1FirstMomIndex+iCnt,
179  iFirstReactionIndex+4, -v.dGet(iCnt));
180 
181  /* termini di Delta_alpha sul nodo 2 */
182  WM.PutItem(18+iCnt, iNode2FirstMomIndex+iCnt,
183  iFirstReactionIndex+4, v.dGet(iCnt));
184  }
185 
186  doublereal d = v.Dot();
187  ASSERT(d > std::numeric_limits<doublereal>::epsilon());
188  if (d > std::numeric_limits<doublereal>::epsilon()) {
189  d = std::sqrt(d);
190  /* termini di Delta_v su alpha */
191  for (int iCnt = 3; iCnt > 0; iCnt--) {
192  WM.PutItem(21+iCnt, iFirstReactionIndex+4,
193  iFirstReactionIndex+iCnt, v.dGet(iCnt)/d);
194  }
195  }
196 
197  return WorkMat;
198 }
199 
200 
201 /* Assemblaggio residuo */
203  doublereal dCoef,
204  const VectorHandler& XCurr,
205  const VectorHandler& /* XPrimeCurr */ )
206 {
207  DEBUGCOUT("Entering DistanceJoint::AssRes()" << std::endl);
208 
209  /* Dimensiona e resetta la matrice di lavoro */
210  integer iNumRows = 0;
211  integer iNumCols = 0;
212  WorkSpaceDim(&iNumRows, &iNumCols);
213  WorkVec.ResizeReset(iNumRows);
214 
215  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
216  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
217  integer iFirstReactionIndex = iGetFirstIndex();
218 
219  for (int iCnt = 1; iCnt <= 3; iCnt++) {
220  /* Indici del nodo 1 */
221  WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex+iCnt);
222 
223  /* Indici del nodo 2 */
224  WorkVec.PutRowIndex(3+iCnt, iNode2FirstMomIndex+iCnt);
225  }
226 
227  /* Indici del vincolo */
228  for (int iCnt = 1; iCnt <= 4; iCnt++) {
229  WorkVec.PutRowIndex(6+iCnt, iFirstReactionIndex+iCnt);
230  }
231 
232  Vec3 x1(pNode1->GetXCurr());
233  Vec3 x2(pNode2->GetXCurr());
234 
235  /* Aggiorna i dati propri */
236  v = Vec3(XCurr, iFirstReactionIndex+1);
237  dAlpha = XCurr(iFirstReactionIndex+4);
238 
239  doublereal dDistance = pGetDriveCaller()->dGet();
240 
241  /* Distanza nulla */
242  if (fabs(dDistance) <= std::numeric_limits<doublereal>::epsilon()) {
243  silent_cerr("DistanceJoint(" << GetLabel() << "): "
244  "near-zero distance" << std::endl);
246  }
247 
248  Vec3 TmpVec(v*dAlpha);
249  WorkVec.Add(1, TmpVec);
250  WorkVec.Sub(4, TmpVec);
251 
252  WorkVec.Add(7, x1-x2+v*dDistance);
253 
254  doublereal d = v.Dot();
255  ASSERT(d > std::numeric_limits<doublereal>::epsilon());
256  if (d > std::numeric_limits<doublereal>::epsilon()) {
257  d = std::sqrt(d);
258  } else {
259  d = 0.;
260  }
261 
262  WorkVec.PutCoef(10, 1.-d);
263 
264  return WorkVec;
265 }
266 
267 
269 {
270  if(bToBeOutput()) {
271  doublereal d = dGet();
272  Vec3 vTmp;
273  if (fabs(d) > std::numeric_limits<doublereal>::epsilon()) {
274  vTmp = Vec3(dAlpha, 0., 0.);
275  } else {
276  vTmp = v;
277  }
278  Joint::Output(OH.Joints(), "Distance", GetLabel(),
279  vTmp, Zero3, v*dAlpha, Zero3)
280  << " " << v << " " << d << std::endl;
281  }
282 }
283 
284 
285 
286 /* Nota: vanno modificati in analogia al DistanceWithOffsetJoint */
287 
288 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
291  const VectorHandler& XCurr)
292 {
293  DEBUGCOUT("Entering DistanceJoint::InitialAssJac()" << std::endl);
294 
295  SparseSubMatrixHandler& WM = WorkMat.SetSparse();
296 
297  /* Dimensiona e resetta la matrice di lavoro */
298  doublereal dDistance = pGetDriveCaller()->dGet();
299 
300  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
301  integer iNode1FirstVelIndex = iNode1FirstPosIndex+6;
302  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
303  integer iNode2FirstVelIndex = iNode2FirstPosIndex+6;
304  integer iFirstReactionIndex = iGetFirstIndex();
305  integer iReactionPrimeIndex = iFirstReactionIndex+4;
306 
307  /* Distanza nulla */
308  /*
309  * forza: dAlpha*v
310  * distanza: x2 - x1 = d*v
311  * equazione di normalizzazione di v:
312  * (v^T*v)^1/2 = 1
313  *
314  *
315  * x1 x1P x2 x2P v a w b
316  *
317  * x1 | 0 0 0 0 -a*I -v 0 0 |
318  * x1P | 0 0 0 0 0 0 -b*I -w |
319  * x2 | 0 0 0 0 a*I v 0 0 |
320  * x2P | 0 0 0 0 0 0 b*I w |
321  * v | -I 0 I 0 -d*I 0 0 0 |
322  * a | 0 0 0 0 r 0 0 0 |
323  * w | 0 0 0 0 -I 0 I 0 |
324  * b | 0 -w^T 0 w^T 0 0 s 0 |
325  *
326  * v^T
327  * r = ----------
328  * (vT*v)^1/2
329  *
330  * s = (x2P - x1P)^T
331  *
332  * a = dAlpha
333  */
334 
335  WM.ResizeReset(51, 1);
336 
337  /* Nota: la direzione della reazione, v,
338  * ed il coefficiente di amplificazione dAlpha
339  * sono stati aggiornati durante il calcolo del residuo */
340 
341  dAlpha = XCurr(iFirstReactionIndex+4);
342  doublereal dBeta = XCurr(iReactionPrimeIndex+4);
343  v = Vec3(XCurr, iFirstReactionIndex+1);
344  Vec3 w(XCurr, iReactionPrimeIndex+1);
345  Vec3 deltaXP(pNode2->GetVCurr()-pNode1->GetVCurr());
346 
347  for (int iCnt = 1; iCnt <= 3; iCnt++) {
348  doublereal d;
349 
350  d = dAlpha;
351  /* termini di Delta_v sul nodo 1 */
352  WM.PutItem(6+iCnt, iNode1FirstPosIndex+iCnt,
353  iFirstReactionIndex+iCnt, -d);
354 
355  /* termini di Delta_v sul nodo 2 */
356  WM.PutItem(9+iCnt, iNode2FirstPosIndex+iCnt,
357  iFirstReactionIndex+iCnt, d);
358 
359  d = v.dGet(iCnt);
360  /* termini di Delta_alpha sul nodo 1 */
361  WM.PutItem(12+iCnt, iNode1FirstPosIndex+iCnt,
362  iFirstReactionIndex+4, -d);
363 
364  /* termini di Delta_alpha sul nodo 2 */
365  WM.PutItem(15+iCnt, iNode2FirstPosIndex+iCnt,
366  iFirstReactionIndex+4, d);
367 
368  d = dBeta;
369  /* termini di Delta_w sul nodo 1 */
370  WM.PutItem(18+iCnt, iNode1FirstVelIndex+iCnt,
371  iReactionPrimeIndex+iCnt, -d);
372 
373  /* termini di Delta_w sul nodo 2 */
374  WM.PutItem(21+iCnt, iNode2FirstVelIndex+iCnt,
375  iReactionPrimeIndex+iCnt, d);
376 
377  d = w.dGet(iCnt);
378  /* termini di Delta_beta sul nodo 1 */
379  WM.PutItem(24+iCnt, iNode1FirstVelIndex+iCnt,
380  iReactionPrimeIndex+4, -d);
381 
382  /* termini di Delta_beta sul nodo 2 */
383  WM.PutItem(27+iCnt, iNode2FirstVelIndex+iCnt,
384  iReactionPrimeIndex+4, d);
385 
386  d = dDistance;
387  /* termini diagonali di Delta_v */
388  WM.PutItem(30+iCnt, iFirstReactionIndex+iCnt,
389  iFirstReactionIndex+iCnt, -d);
390 
391  /* termini diagonali di Delta_w */
392  WM.PutItem(33+iCnt, iReactionPrimeIndex+iCnt,
393  iReactionPrimeIndex+iCnt, 1.);
394 
395  /* termini incrociati di Delta_w Delta_v */
396  WM.PutItem(36+iCnt, iReactionPrimeIndex+iCnt,
397  iFirstReactionIndex+iCnt, -1.);
398 
399  d = w.dGet(iCnt);
400  /* termini di beta per Delta_x1P: w^T */
401  WM.PutItem(39+iCnt, iReactionPrimeIndex+4,
402  iNode1FirstVelIndex+iCnt, -d);
403 
404  /* termini di beta per Delta_x2P: w^T */
405  WM.PutItem(42+iCnt, iReactionPrimeIndex+4,
406  iNode2FirstVelIndex+iCnt, d);
407 
408  d = deltaXP.dGet(iCnt);
409  /* termini di beta per Delta_w: (x2P-x1P)^T */
410  WM.PutItem(45+iCnt, iReactionPrimeIndex+4,
411  iReactionPrimeIndex+iCnt, d);
412  }
413 
414 
415  /* termini di Delta_v su alpha */
416  doublereal d = v.Dot();
417  ASSERT(d > std::numeric_limits<doublereal>::epsilon());
418  if (d > std::numeric_limits<doublereal>::epsilon()) {
419  d = std::sqrt(d);
420  for (int iCnt = 1; iCnt <= 3; iCnt++) {
421  WM.PutItem(48+iCnt, iFirstReactionIndex+4,
422  iFirstReactionIndex+iCnt, v.dGet(iCnt)/d);
423  }
424  } else {
425  for (int iCnt = 1; iCnt <= 3; iCnt++) {
426  WM.PutItem(48+iCnt, iFirstReactionIndex+4,
427  iFirstReactionIndex+iCnt, 0.);
428  }
429  }
430 
431  for (int iCnt = 1; iCnt <= 3; iCnt++) {
432  /* termini di Delta_x1 */
433  WM.PutItem(iCnt, iFirstReactionIndex+iCnt,
434  iNode1FirstPosIndex+iCnt, -1.);
435 
436  /* termini di Delta_x2 */
437  WM.PutItem(iCnt+3, iFirstReactionIndex+iCnt,
438  iNode2FirstPosIndex+iCnt, 1.);
439  }
440 
441  return WorkMat;
442 }
443 
444 
445 /* Contributo al residuo durante l'assemblaggio iniziale */
448  const VectorHandler& XCurr)
449 {
450  DEBUGCOUT("Entering DistanceJoint::InitialAssRes()" << std::endl);
451 
452  /* Dimensiona e resetta la matrice di lavoro */
453  WorkVec.ResizeReset(20);
454 
455  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
456  integer iNode1FirstVelIndex = iNode1FirstPosIndex+6;
457  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
458  integer iNode2FirstVelIndex = iNode2FirstPosIndex+6;
459  integer iFirstReactionIndex = iGetFirstIndex();
460  integer iReactionPrimeIndex = iFirstReactionIndex+4;
461 
462  for (int iCnt = 1; iCnt <= 3; iCnt++) {
463  /* Indici del nodo 1, posizione */
464  WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
465 
466  /* Indici del nodo 1, velocita' */
467  WorkVec.PutRowIndex(3+iCnt, iNode1FirstVelIndex+iCnt);
468 
469  /* Indici del nodo 2, posizione */
470  WorkVec.PutRowIndex(6+iCnt, iNode2FirstPosIndex+iCnt);
471 
472  /* Indici del nodo 2, velocita' */
473  WorkVec.PutRowIndex(9+iCnt, iNode2FirstVelIndex+iCnt);
474  }
475 
476  /* Indici del vincolo */
477  for (int iCnt = 1; iCnt <= 8; iCnt++) {
478  WorkVec.PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
479  }
480 
481  Vec3 x1(pNode1->GetXCurr());
482  Vec3 x2(pNode2->GetXCurr());
483  Vec3 x1P(pNode1->GetVCurr());
484  Vec3 x2P(pNode2->GetVCurr());
485  v = Vec3(XCurr, iFirstReactionIndex+1);
486  Vec3 w(XCurr, iReactionPrimeIndex+1);
487  dAlpha = XCurr(iFirstReactionIndex+4);
488  doublereal dBeta = XCurr(iReactionPrimeIndex+4);
489 
490  doublereal dDistance = pGetDriveCaller()->dGet();
491 
492  /* Distanza nulla */
493  if (fabs(dDistance) <= std::numeric_limits<doublereal>::epsilon()) {
494  silent_cerr("DistanceJoint(" << GetLabel() << "): "
495  "near-zero distance" << std::endl);
497  }
498  WorkVec.Add(0+1, v*dAlpha);
499  WorkVec.Add(3+1, w*dBeta);
500  WorkVec.Add(6+1, -v*dAlpha);
501  WorkVec.Add(9+1, -w*dBeta);
502  WorkVec.Add(12+1, x1-x2+v*dDistance);
503 
504  doublereal d = v.Dot();
505  ASSERT(d > std::numeric_limits<doublereal>::epsilon());
506  if(d > std::numeric_limits<doublereal>::epsilon()) {
507  d = std::sqrt(d);
508  } else {
509  d = 0.;
510  }
511 
512  WorkVec.PutCoef(15+1, 1.-d);
513 
514  WorkVec.Add(16+1, v-w);
515  WorkVec.PutCoef(19+1, (x1P-x2P).Dot(w));
516 
517  return WorkVec;
518 }
519 
520 
522 {
523  integer iFirstIndex = iGetFirstIndex();
524 
525  doublereal dDistance = pGetDriveCaller()->dGet();
526  if (fabs(dDistance) <= std::numeric_limits<doublereal>::epsilon()) {
527  silent_cerr("DistanceJoint(" << GetLabel() << "): "
528  "near-zero distance" << std::endl);
530  }
531 
532  v = ((pNode2->GetXCurr())-(pNode1->GetXCurr()));
533  doublereal d = v.Dot();
534  if (d < std::numeric_limits<doublereal>::epsilon()) {
535  silent_cerr("DistanceJoint(" << GetLabel() << "): "
536  "initial length is null" << std::endl);
538  }
539  v /= std::sqrt(d);
540 
541  X.Put(iFirstIndex+1, v);
542  X.Put(iFirstIndex+4+1, v);
543 }
544 
545 
546 void
548  VectorHandler& X, VectorHandler& /* XP */ ,
550 {
551  if (ph) {
552  for (unsigned i = 0; i < ph->size(); i++) {
553  DriveHint *pdh = dynamic_cast<DriveHint *>((*ph)[i]);
554 
555  if (pdh) {
556  pedantic_cout("DistanceJoint(" << uLabel << "): "
557  "creating drive from hint[" << i << "]..." << std::endl);
558 
559  DriveCaller *pDC = pdh->pCreateDrive(pDM);
560  if (pDC == 0) {
561  silent_cerr("DistanceJoint(" << uLabel << "): "
562  "unable to create drive after hint "
563  "#" << i << std::endl);
565  }
566 
567  DriveOwner::Set(pDC);
568  continue;
569  }
570  }
571  }
572 
573  doublereal dDistance = pGetDriveCaller()->dGet();
574 
575  /* Setta a 1 dAlpha, che e' indipendente dalle altre variabili
576  * in caso di distanza nulla */
577  if (fabs(dDistance) <= std::numeric_limits<doublereal>::epsilon()) {
578  silent_cerr("DistanceJoint(" << uLabel << "): "
579  "near-zero distance" << std::endl);
581  }
582 
583  /* Scrive la direzione della distanza. Se e' stata ottenuta con
584  * l'assemblaggio iniziale bene, se no' la calcola */
585  v = pNode2->GetXCurr() - pNode1->GetXCurr();
586  doublereal d = v.Dot();
587  if (d <= std::numeric_limits<doublereal>::epsilon()) {
588  silent_cerr("DistanceJoint(" << uLabel << ") "
589  "linked to nodes " << pNode1->GetLabel()
590  << " and " << pNode2->GetLabel() << ": "
591  "nodes are coincident;" << std::endl
592  << "initial joint assembly is recommended"
593  << std::endl);
595  }
596  v /= std::sqrt(d);
597 
598  /* Scrittura sul vettore soluzione */
599  X.Put(iGetFirstIndex() + 1, v);
600 }
601 
602 /* DistanceJoint - end */
603 
604 
605 /* DistanceJointWithOffset - begin */
606 
607 /* Costruttore non banale */
609  const DofOwner* pDO,
610  const StructNode* pN1,
611  const StructNode* pN2,
612  const Vec3& f1Tmp,
613  const Vec3& f2Tmp,
614  const DriveCaller* pDC,
615  flag fOut)
616 : Elem(uL, fOut),
617 Joint(uL, pDO, fOut),
618 DriveOwner(pDC),
619 pNode1(pN1), pNode2(pN2), f1(f1Tmp), f2(f2Tmp), v(Zero3), dAlpha(0.)
620 {
621  ASSERT(pDO != NULL);
622  ASSERT(pDC != NULL);
623  ASSERT(pNode1 != NULL);
625  ASSERT(pNode2 != NULL);
627 }
628 
629 
630 /* Distruttore banale - ci pensa il DriveOwner a distruggere il DriveCaller */
632 {
633  NO_OP;
634 }
635 
636 /* Dati privati */
637 unsigned int
639 {
640  return 1;
641 }
642 
643 unsigned int
645 {
646  ASSERT(s != NULL);
647 
648  if (strcmp(s, "d") == 0) {
649  return 1;
650  }
651 
652  return 0;
653 }
654 
657 {
658  ASSERT(i == 1);
659 
660  if (i == 1) {
661  return dGet();
662  }
663 
665 }
666 
667 
668 /* Contributo al file di restart */
669 std::ostream& DistanceJointWithOffset::Restart(std::ostream& out) const
670 {
671  Joint::Restart(out) << ", distance with offset, "
672  << pNode1->GetLabel()
673  << ", reference, node, ",
674  f1.Write(out, ", ") << ", "
675  << pNode2->GetLabel()
676  << ", reference, node, ",
677  f2.Write(out, ", ") << ", ";
678  return pGetDriveCaller()->Restart(out) << ';' << std::endl;
679 }
680 
681 
682 /* Assemblaggio jacobiano */
685  doublereal dCoef,
686  const VectorHandler& /* XCurr */ ,
687  const VectorHandler& /* XPrimeCurr */ )
688 {
689  DEBUGCOUT("Entering DistanceJointWithOffset::AssJac()" << std::endl);
690 
691  SparseSubMatrixHandler& WM = WorkMat.SetSparse();
692 
693  /* Dimensiona e resetta la matrice di lavoro */
694  doublereal dDistance = pGetDriveCaller()->dGet();
695 
696  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
697  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
698  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
699  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
700  integer iFirstReactionIndex = iGetFirstIndex();
701 
702  Vec3 f1Tmp(pNode1->GetRRef()*f1);
703  Vec3 f2Tmp(pNode2->GetRRef()*f2);
704 
705  /* Distanza nulla */
706  WM.ResizeReset(72, 0);
707 
708  for (int iCnt = 1; iCnt <= 3; iCnt++) {
709 
710  /* termini di Delta_x1 */
711  WM.PutItem(iCnt, iFirstReactionIndex+iCnt,
712  iNode1FirstPosIndex+iCnt, -dCoef);
713 
714  /* termini di Delta_x2 */
715  WM.PutItem(3+iCnt, iFirstReactionIndex+iCnt,
716  iNode2FirstPosIndex+iCnt, dCoef);
717 
718  /* termini di Delta_F sul nodo 1 */
719  WM.PutItem(6+iCnt, iNode1FirstMomIndex+iCnt,
720  iFirstReactionIndex+iCnt, -dAlpha);
721 
722  /* termini di Delta_F sul nodo 2 */
723  WM.PutItem(9+iCnt, iNode2FirstMomIndex+iCnt,
724  iFirstReactionIndex+iCnt, dAlpha);
725 
726  /* termini diagonali di Delta_v */
727  WM.PutItem(12+iCnt, iFirstReactionIndex+iCnt,
728  iFirstReactionIndex+iCnt, -dDistance);
729 
730  doublereal d = v.dGet(iCnt);
731 
732  /* termini di Delta_alpha sul nodo 1 */
733  WM.PutItem(15+iCnt, iNode1FirstMomIndex+iCnt,
734  iFirstReactionIndex+4, -d);
735 
736  /* termini di Delta_alpha sul nodo 2 */
737  WM.PutItem(18+iCnt, iNode2FirstMomIndex+iCnt,
738  iFirstReactionIndex+4, d);
739  }
740 
741 
742  /* Termini di offset nell'equazione di vincolo */
743  WM.PutCross(22, iFirstReactionIndex,
744  iNode1FirstPosIndex+3, f1Tmp*dCoef);
745 
746  WM.PutCross(28, iFirstReactionIndex,
747  iNode2FirstPosIndex+3, f2Tmp*(-dCoef));
748 
749 
750  Vec3 Tmp(v*(dAlpha*dCoef));
751 
752  WM.PutMat3x3(34, iNode1FirstMomIndex+3,
753  iNode1FirstPosIndex+3, Mat3x3(MatCrossCross, Tmp, -f1Tmp));
754  WM.PutMat3x3(43, iNode2FirstMomIndex+3,
755  iNode2FirstPosIndex+3, Mat3x3(MatCrossCross, Tmp, f2Tmp));
756 
757 
758  WM.PutCross(52, iNode1FirstMomIndex+3,
759  iFirstReactionIndex, f1Tmp*(-dAlpha));
760  WM.PutCross(58, iNode2FirstMomIndex+3,
761  iFirstReactionIndex, f2Tmp*dAlpha);
762 
763  Tmp = v.Cross(f1Tmp);
764  for (int iCnt = 1; iCnt <= 3; iCnt++) {
765  WM.PutItem(63+iCnt, iNode1FirstMomIndex+3+iCnt,
766  iFirstReactionIndex+4, Tmp.dGet(iCnt));
767  }
768 
769  Tmp = f2Tmp.Cross(v);
770  for (int iCnt = 1; iCnt <= 3; iCnt++) {
771  WM.PutItem(66+iCnt, iNode2FirstMomIndex+3+iCnt,
772  iFirstReactionIndex+4, Tmp.dGet(iCnt));
773  }
774 
775 
776  doublereal d = v.Dot();
777  ASSERT(d > std::numeric_limits<doublereal>::epsilon());
778  if (d > std::numeric_limits<doublereal>::epsilon()) {
779  d = std::sqrt(d);
780  } else {
781  d = 1.;
782  }
783 
784  /* termini di Delta_v su alpha */
785  for (int iCnt = 1; iCnt <= 3; iCnt++) {
786  WM.PutItem(69+iCnt, iFirstReactionIndex+4,
787  iFirstReactionIndex+iCnt, v.dGet(iCnt)/d);
788  }
789 
790  return WorkMat;
791 }
792 
793 
794 /* Assemblaggio residuo */
797  doublereal dCoef,
798  const VectorHandler& XCurr,
799  const VectorHandler& /* XPrimeCurr */ )
800 {
801  DEBUGCOUT("Entering DistanceJointWithOffset::AssRes()" << std::endl);
802 
803  /* Dimensiona e resetta la matrice di lavoro */
804  integer iNumRows = 0;
805  integer iNumCols = 0;
806  WorkSpaceDim(&iNumRows, &iNumCols);
807  WorkVec.ResizeReset(iNumRows);
808 
809  integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
810  integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
811  integer iFirstReactionIndex = iGetFirstIndex();
812 
813  for (int iCnt = 1; iCnt <= 6; iCnt++) {
814  /* Indici del nodo 1 */
815  WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex+iCnt);
816  WorkVec.PutRowIndex(6+iCnt, iNode2FirstMomIndex+iCnt);
817  }
818 
819  /* Indici del vincolo */
820  for(int iCnt = 1; iCnt <= 4; iCnt++) {
821  WorkVec.PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
822  }
823 
824  Vec3 x1(pNode1->GetXCurr());
825  Vec3 x2(pNode2->GetXCurr());
826  Vec3 f1Tmp(pNode1->GetRCurr()*f1);
827  Vec3 f2Tmp(pNode2->GetRCurr()*f2);
828 
829  /* Aggiorna i dati propri */
830  v = Vec3(XCurr, iFirstReactionIndex+1);
831  dAlpha = XCurr(iFirstReactionIndex+4);
832 
833  doublereal dDistance = pGetDriveCaller()->dGet();
834 
835  /* Distanza nulla */
836  if (fabs(dDistance) <= std::numeric_limits<doublereal>::epsilon()) {
837  silent_cerr("DistanceJoint(" << GetLabel() << "): "
838  "near-zero distance" << std::endl);
840  }
841  Vec3 TmpVec(v*dAlpha);
842  WorkVec.Add(1, TmpVec);
843  WorkVec.Add(4, f1Tmp.Cross(TmpVec));
844  WorkVec.Sub(7, TmpVec);
845  WorkVec.Sub(10, f2Tmp.Cross(TmpVec));
846 
847  WorkVec.Add(13, x1+f1Tmp-x2-f2Tmp+v*dDistance);
848 
849  doublereal dTmp = v.Dot();
850  ASSERT(dTmp >= 0.0);
851  dTmp = std::sqrt(dTmp);
852 
853  WorkVec.PutCoef(16, 1.-dTmp);
854 
855  return WorkVec;
856 }
857 
858 
860 {
861  if (bToBeOutput()) {
862  doublereal d = dGet();
863  Vec3 vTmp;
864  if (fabs(d) > std::numeric_limits<doublereal>::epsilon()) {
865  vTmp = Vec3(dAlpha, 0., 0.);
866  } else {
867  vTmp = v;
868  }
869  Joint::Output(OH.Joints(), "DistanceWithOffs", GetLabel(),
870  vTmp, Zero3, v*dAlpha, Zero3)
871  << " " << v << " " << d << std::endl;
872  }
873 }
874 
875 
876 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
879  const VectorHandler& XCurr)
880 {
881  DEBUGCOUT("Entering DistanceJointWithOffset::InitialAssJac()" << std::endl);
882 
883 
884  FullSubMatrixHandler& WM = WorkMat.SetFull();
885  WM.ResizeReset(32, 32);
886 
887  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
888  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
889  integer iFirstReactionIndex = iGetFirstIndex();
890  integer iReactionPrimeIndex = iFirstReactionIndex+4;
891 
892  for (int iCnt = 1; iCnt <= 12; iCnt++) {
893  WM.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
894  WM.PutColIndex(iCnt, iNode1FirstPosIndex+iCnt);
895  WM.PutRowIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
896  WM.PutColIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
897  }
898 
899  for (int iCnt = 1; iCnt <= 8; iCnt++) {
900  WM.PutRowIndex(24+iCnt, iFirstReactionIndex+iCnt);
901  WM.PutColIndex(24+iCnt, iFirstReactionIndex+iCnt);
902  }
903 
904  doublereal dAlphaP = XCurr(iReactionPrimeIndex+4);
905  Vec3 vP(XCurr, iReactionPrimeIndex+1);
906 
907  Vec3 f1Tmp(pNode1->GetRRef()*f1);
908  Vec3 f2Tmp(pNode2->GetRRef()*f2);
909  Vec3 Omega1(pNode1->GetWRef());
910  Vec3 Omega2(pNode2->GetWRef());
911 
912  doublereal dDistance = pGetDriveCaller()->dGet();
913 
914  /* Equazioni di equilibrio */
915  for (int iCnt = 1; iCnt <= 3; iCnt++) {
916  WM.PutCoef(iCnt, 24+iCnt, -dAlpha);
917  WM.PutCoef(12+iCnt, 24+iCnt, dAlpha);
918 
919  WM.PutCoef(6+iCnt, 28+iCnt, -dAlpha);
920  WM.PutCoef(18+iCnt, 28+iCnt, dAlpha);
921 
922  WM.PutCoef(6+iCnt, 24+iCnt, -dAlphaP);
923  WM.PutCoef(18+iCnt, 24+iCnt, dAlphaP);
924 
925  doublereal d = v.dGet(iCnt);
926  WM.PutCoef(iCnt, 28, -d);
927  WM.PutCoef(12+iCnt, 28, d);
928 
929  WM.PutCoef(6+iCnt, 32, -d);
930  WM.PutCoef(18+iCnt, 32, d);
931 
932  d = vP.dGet(iCnt);
933  WM.PutCoef(6+iCnt, 28, -d);
934  WM.PutCoef(18+iCnt, 28, d);
935 
936  }
937 
938  Vec3 Tmp(v.Cross(f1Tmp));
939  for (int iCnt = 1; iCnt <= 3; iCnt++) {
940  doublereal d = Tmp.dGet(iCnt);
941  WM.PutCoef(3+iCnt, 28, d);
942  WM.PutCoef(9+iCnt, 32, d);
943  }
944 
945  Tmp = f2Tmp.Cross(v);
946  for (int iCnt = 1; iCnt <= 3; iCnt++) {
947  doublereal d = Tmp.dGet(iCnt);
948  WM.PutCoef(15+iCnt, 28, d);
949  WM.PutCoef(21+iCnt, 32, d);
950  }
951 
952  Tmp = v.Cross(Omega1.Cross(f1Tmp))+vP.Cross(f1Tmp);
953  for (int iCnt = 1; iCnt <= 3; iCnt++) {
954  doublereal d = Tmp.dGet(iCnt);
955  WM.PutCoef(9+iCnt, 28, d);
956  }
957 
958  Tmp = v.Cross(f2Tmp.Cross(Omega2))-vP.Cross(f2Tmp);
959  for (int iCnt = 1; iCnt <= 3; iCnt++) {
960  doublereal d = Tmp.dGet(iCnt);
961  WM.PutCoef(21+iCnt, 28, d);
962  }
963 
964 
965  Tmp = f1Tmp*(-dAlpha);
966 
967  Mat3x3 MTmp(MatCrossCross, v, Tmp);
968  WM.Add(4, 4, MTmp);
969  WM.Add(10, 10, MTmp);
970 
971  MTmp = Mat3x3(MatCross, Tmp);
972  WM.Add(4, 25, MTmp);
973  WM.Add(10, 29, MTmp);
974 
975  Tmp = f2Tmp*dAlpha;
976  MTmp = Mat3x3(MatCrossCross, v, Tmp);
977  WM.Add(16, 16, MTmp);
978  WM.Add(22, 22, MTmp);
979 
980  MTmp = Mat3x3(MatCross, Tmp);
981  WM.Add(16, 25, MTmp);
982  WM.Add(22, 29, MTmp);
983 
984  MTmp = (Mat3x3(MatCrossCross, v*dAlpha, Omega1) + Mat3x3(MatCross, vP*dAlpha + v*dAlphaP))*Mat3x3(MatCross, f1Tmp);
985  WM.Sub(10, 4, MTmp);
986  MTmp = Mat3x3(MatCross, f1Tmp.Cross(Omega1*dAlpha) - f1Tmp*dAlphaP);
987  WM.Add(10, 25, MTmp);
988 
989  MTmp = (Mat3x3(MatCrossCross, v*dAlpha, Omega2) + Mat3x3(MatCross, vP*dAlpha + v*dAlphaP))*Mat3x3(MatCross, f2Tmp);
990  WM.Add(22, 16, MTmp);
991  MTmp = Mat3x3(MatCross, f2Tmp.Cross(Omega2*(-dAlpha)) + f2Tmp*dAlphaP);
992  WM.Add(22, 25, MTmp);
993 
994  /* Equazioni di vincolo */
995  for (int iCnt = 1; iCnt <= 3; iCnt++) {
996  WM.PutCoef(24+iCnt, iCnt, -1.);
997  WM.PutCoef(24+iCnt, 12+iCnt, 1.);
998 
999  WM.PutCoef(24+iCnt, 24+iCnt, -dDistance);
1000 
1001  WM.PutCoef(28+iCnt, 6+iCnt, -1.);
1002  WM.PutCoef(28+iCnt, 18+iCnt, 1.);
1003 
1004  WM.PutCoef(28+iCnt, 28+iCnt, -dDistance);
1005  }
1006 
1007  MTmp = Mat3x3(MatCross, f1Tmp);
1008  WM.Add(25, 4, MTmp);
1009  WM.Add(29, 10, MTmp);
1010 
1011  MTmp = Mat3x3(MatCross, f2Tmp);
1012  WM.Sub(25, 16, MTmp);
1013  WM.Sub(29, 22, MTmp);
1014 
1015  MTmp = Mat3x3(MatCrossCross, Omega1, f1Tmp);
1016  WM.Add(29, 4, MTmp);
1017 
1018  MTmp = Mat3x3(MatCrossCross, Omega2, f2Tmp);
1019  WM.Sub(29, 16, MTmp);
1020 
1021  doublereal d = v.Dot();
1022  ASSERT(d > std::numeric_limits<doublereal>::epsilon());
1023  if (d > std::numeric_limits<doublereal>::epsilon()) {
1024  d = std::sqrt(d);
1025 
1026  Tmp = v/d;
1027  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1028  doublereal d = Tmp.dGet(iCnt);
1029  WM.PutCoef(28, 24+iCnt, d);
1030  WM.PutCoef(32, 28+iCnt, d);
1031  }
1032 
1033  Tmp = vP/d-v*((vP.Dot(v))/pow(d, 3));
1034  for (int iCnt = 1; iCnt <= 3; iCnt++) {
1035  doublereal d = Tmp.dGet(iCnt);
1036  WM.PutCoef(32, 24+iCnt, d);
1037  }
1038  }
1039 
1040  return WorkMat;
1041 }
1042 
1043 
1044 /* Contributo al residuo durante l'assemblaggio iniziale */
1047  const VectorHandler& XCurr)
1048 {
1049  DEBUGCOUT("Entering DistanceJointWithOffset::InitialAssRes()" << std::endl);
1050 
1051  /* Dimensiona e resetta la matrice di lavoro */
1052  WorkVec.ResizeReset(32);
1053 
1054  integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
1055  integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
1056  integer iFirstReactionIndex = iGetFirstIndex();
1057  integer iReactionPrimeIndex = iFirstReactionIndex+4;
1058 
1059  for (int iCnt = 1; iCnt <= 12; iCnt++) {
1060  WorkVec.PutRowIndex(iCnt, iNode1FirstPosIndex+iCnt);
1061  WorkVec.PutRowIndex(12+iCnt, iNode2FirstPosIndex+iCnt);
1062  }
1063 
1064  for (int iCnt = 1; iCnt <= 8; iCnt++) {
1065  WorkVec.PutRowIndex(24+iCnt, iFirstReactionIndex+iCnt);
1066  }
1067 
1068  v = Vec3(XCurr, iFirstReactionIndex+1);
1069  Vec3 vP(XCurr, iReactionPrimeIndex+1);
1070  dAlpha = XCurr(iFirstReactionIndex+4);
1071  doublereal dAlphaP = XCurr(iReactionPrimeIndex+4);
1072 
1073  Vec3 x1(pNode1->GetXCurr());
1074  Vec3 x2(pNode2->GetXCurr());
1075  Vec3 v1(pNode1->GetVCurr());
1076  Vec3 v2(pNode2->GetVCurr());
1077  Vec3 f1Tmp(pNode1->GetRCurr()*f1);
1078  Vec3 f2Tmp(pNode2->GetRCurr()*f2);
1079  Vec3 Omega1(pNode1->GetWCurr());
1080  Vec3 Omega2(pNode2->GetWCurr());
1081 
1082  doublereal dDistance = pGetDriveCaller()->dGet();
1083 
1084  if (fabs(dDistance) <= std::numeric_limits<doublereal>::epsilon()) {
1085  silent_cerr("DistanceJoint(" << GetLabel() << "): "
1086  "near-zero distance" << std::endl);
1088  }
1089  Vec3 Tmp(v*dAlpha);
1090  WorkVec.Put(1, Tmp);
1091  WorkVec.Put(4, f1Tmp.Cross(Tmp));
1092  WorkVec.Put(13, -Tmp);
1093  WorkVec.Put(16, Tmp.Cross(f2Tmp));
1094 
1095  Tmp = vP*dAlpha+v*dAlphaP;
1096  WorkVec.Put(7, Tmp);
1097  WorkVec.Put(10, (Omega1.Cross(f1Tmp)).Cross(v*dAlpha)+f1Tmp.Cross(Tmp));
1098  WorkVec.Put(19, -Tmp);
1099  WorkVec.Put(22, (f2Tmp.Cross(Omega2)).Cross(v*dAlpha)+Tmp.Cross(f2Tmp));
1100 
1101  WorkVec.Put(25, v*dDistance-x2-f2Tmp+x1+f1Tmp);
1102  WorkVec.Put(29, vP*dDistance
1103  -v2-Omega2.Cross(f2Tmp)+v1+Omega1.Cross(f1Tmp));
1104 
1105  doublereal d = v.Dot();
1106 
1107  ASSERT(d > std::numeric_limits<doublereal>::epsilon());
1108  d = std::sqrt(d);
1109  WorkVec.PutCoef(28, 1.-d);
1110  WorkVec.PutCoef(32, -v.Dot(vP)/d);
1111 
1112  return WorkVec;
1113 }
1114 
1115 
1117 {
1118  integer iFirstIndex = iGetFirstIndex();
1119 
1120  doublereal dDistance = pGetDriveCaller()->dGet();
1121  if (fabs(dDistance) <= std::numeric_limits<doublereal>::epsilon()) {
1122  silent_cerr("DistanceJoint(" << GetLabel() << "): "
1123  "near-zero distance" << std::endl);
1125  }
1126 
1127  Vec3 x1(pNode1->GetXCurr());
1128  Vec3 x2(pNode2->GetXCurr());
1129  Vec3 v1(pNode1->GetVCurr());
1130  Vec3 v2(pNode2->GetVCurr());
1131  Vec3 f1Tmp((pNode1->GetRCurr())*f1);
1132  Vec3 f2Tmp((pNode2->GetRCurr())*f2);
1133  Vec3 Omega1(pNode1->GetWCurr());
1134  Vec3 Omega2(pNode2->GetWCurr());
1135 
1136  v = x2+f2Tmp-x1-f1Tmp;
1137  doublereal d = v.Dot();
1138  if (d < std::numeric_limits<doublereal>::epsilon()) {
1139  silent_cerr("DistanceJoint(" << GetLabel() << "): "
1140  "initial length is null" << std::endl);
1142  }
1143  v /= std::sqrt(d);
1144 
1145  X.Put(iFirstIndex+1, v);
1146  X.Put(iFirstIndex+5, (v2+Omega2.Cross(f2Tmp)-v1-Omega1.Cross(f1Tmp))/d);
1147 }
1148 
1149 
1150 void
1152  VectorHandler& X, VectorHandler& /* XP */ ,
1154 {
1155  if (ph) {
1156  for (unsigned i = 0; i < ph->size(); i++) {
1157  pedantic_cout("DistanceJointWithOffset(" << uLabel << "): "
1158  "creating drive from hint..." << std::endl);
1159 
1160  DriveHint *pdh = dynamic_cast<DriveHint *>((*ph)[i]);
1161 
1162  if (pdh) {
1163  DriveCaller *pDC = pdh->pCreateDrive(pDM);
1164  if (pDC == 0) {
1165  silent_cerr("DistanceJointWithOffset(" << uLabel << "): "
1166  "unable to create drive after hint "
1167  "#" << i << std::endl);
1169  }
1170 
1171  DriveOwner::Set(pDC);
1172  continue;
1173  }
1174  }
1175  }
1176 
1177  doublereal dDistance = pGetDriveCaller()->dGet();
1178 
1179  /* Setta a 1 dAlpha, che e' indipendente dalle altre variabili
1180  * in caso di distanza nulla */
1181  if (fabs(dDistance) <= std::numeric_limits<doublereal>::epsilon()) {
1182  silent_cerr("DistanceJoint(" << GetLabel() << "):"
1183  "near-zero distance" << std::endl);
1185  }
1186 
1188  doublereal d = v.Dot();
1189  if (d <= std::numeric_limits<doublereal>::epsilon()) {
1190  silent_cerr("DistanceJoint(" << GetLabel() << ") "
1191  "linked to nodes " << pNode1->GetLabel()
1192  << " and " << pNode2->GetLabel() << ": "
1193  "nodes are coincident;" << std::endl
1194  << "this is no longer supported" << std::endl);
1196  }
1197  v /= std::sqrt(d);
1198 
1199  /* Scrittura sul vettore soluzione */
1200  X.Put(iGetFirstIndex() + 1, v);
1201 }
1202 
1203 
1204 /* DistanceJointWithOffset - end */
1205 
1206 #endif
1207 
1208 
1209 /* ClampJoint - begin */
1210 
1211 /* Costruttore definitivo (da mettere a punto) */
1212 ClampJoint::ClampJoint(unsigned int uL, const DofOwner*pD,
1213  const StructNode* pN,
1214  const Vec3& X0, const Mat3x3& R0,
1215  flag fOut)
1216 : Elem(uL, fOut),
1217 Joint(uL, pD, fOut),
1218 pNode(pN), XClamp(X0), RClamp(R0), F(Zero3), M(Zero3)
1219 {
1220  NO_OP;
1221 }
1222 
1223 
1225 {
1226  NO_OP;
1227 }
1228 
1229 
1230 std::ostream&
1231 ClampJoint::DescribeDof(std::ostream& out, const char *prefix, bool bInitial) const
1232 {
1233  integer iIndex = iGetFirstIndex();
1234 
1235  out
1236  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
1237  "reaction forces [Fx,Fy,Fz]" << std::endl
1238  << prefix << iIndex + 4 << "->" << iIndex + 6 << ": "
1239  "reaction couples [mx,my,mz]" << std::endl;
1240 
1241  if (bInitial) {
1242  iIndex += 6;
1243  out
1244  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
1245  "reaction force derivatives [FPx,FPy,FPz]" << std::endl
1246  << prefix << iIndex + 4 << "->" << iIndex + 6 << ": "
1247  "reaction couple derivatives [mPx,mPy,mPz]" << std::endl;
1248  }
1249 
1250  return out;
1251 }
1252 
1253 static const char xyz[] = "xyz";
1254 static const char *dof[] = {
1255  "reaction force f",
1256  "reaction couple m",
1257  "reaction force derivative fP",
1258  "reaction couple derivative mP"
1259 };
1260 static const char *eq[] = {
1261  "position constraint P",
1262  "orientation constraint theta",
1263  "position constraint derivative v",
1264  "orientation constraint derivative w"
1265 };
1266 
1267 void
1268 ClampJoint::DescribeDof(std::vector<std::string>& desc,
1269  bool bInitial, int i) const
1270 {
1271  int iend = 1;
1272  if (i == -1) {
1273  if (bInitial) {
1274  iend = 12;
1275 
1276  } else {
1277  iend = 6;
1278  }
1279  }
1280  desc.resize(iend);
1281 
1282  std::ostringstream os;
1283  os << "ClampJoint(" << GetLabel() << ")";
1284 
1285  if (i == -1) {
1286  std::string name = os.str();
1287  for (i = 0; i < iend; i++) {
1288  os.str(name);
1289  os.seekp(0, std::ios_base::end);
1290  os << ": " << dof[i/3] << xyz[i%3];
1291 
1292  desc[i] = os.str();
1293  }
1294 
1295  } else {
1296  os << ": " << dof[i/3] << xyz[i%3];
1297  desc[0] = os.str();
1298  }
1299 }
1300 
1301 std::ostream&
1302 ClampJoint::DescribeEq(std::ostream& out, const char *prefix, bool bInitial) const
1303 {
1304  integer iIndex = iGetFirstIndex();
1305 
1306  out
1307  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
1308  "position constraints [Px=Px0,Py=Py0,Pz=Pz0]" << std::endl
1309  << prefix << iIndex + 4 << "->" << iIndex + 6 << ": "
1310  "orientation constraints [thetax=thetax0,thetay=thetay0,thetaz=thetaz0]" << std::endl;
1311 
1312  if (bInitial) {
1313  iIndex += 6;
1314  out
1315  << prefix << iIndex + 1 << "->" << iIndex + 3 << ": "
1316  "velocity constraints [vx=0,vy=0,vz=0]" << std::endl
1317  << prefix << iIndex + 4 << "->" << iIndex + 6 << ": "
1318  "angular velocity constraints [wx=0,wy=0,wz=0]" << std::endl;
1319  }
1320 
1321  return out;
1322 }
1323 
1324 void
1325 ClampJoint::DescribeEq(std::vector<std::string>& desc,
1326  bool bInitial, int i) const
1327 {
1328  int iend = 1;
1329  if (i == -1) {
1330  if (bInitial) {
1331  iend = 12;
1332 
1333  } else {
1334  iend = 6;
1335  }
1336  }
1337  desc.resize(iend);
1338 
1339  std::ostringstream os;
1340  os << "ClampJoint(" << GetLabel() << ")";
1341 
1342  if (i == -1) {
1343  std::string name = os.str();
1344  for (i = 0; i < iend; i++) {
1345  os.str(name);
1346  os.seekp(0, std::ios_base::end);
1347  os << ": " << eq[i/3] << xyz[i%3];
1348 
1349  desc[i] = os.str();
1350  }
1351 
1352  } else {
1353  os << ": " << eq[i/3] << xyz[i%3];
1354  desc[0] = os.str();
1355  }
1356 }
1357 
1358 /*Funzione che legge lo stato iniziale dal file di input*/
1359 void
1361 {
1362  F = HP.GetVec3();
1363  M = HP.GetVec3();
1364 }
1365 
1366 /* Contributo al file di restart */
1367 std::ostream&
1368 ClampJoint::Restart(std::ostream& out) const
1369 {
1370  return Joint::Restart(out) << ", clamp, "
1371  << pNode->GetLabel() << ", node, node, "
1372  << "initial state, ", F.Write(out, ", ")
1373  << ", ", M.Write(out, ", ") << ';' << std::endl;
1374 }
1375 
1376 
1379  doublereal /* dCoef */ ,
1380  const VectorHandler& /* XCurr */ ,
1381  const VectorHandler& /* XPrimeCurr */ )
1382 {
1383  DEBUGCOUT("Entering ClampJoint::AssJac()" << std::endl);
1384 
1385  SparseSubMatrixHandler& WM = WorkMat.SetSparse();
1386 
1387  /* Dimensiona e resetta la matrice di lavoro */
1388  WM.ResizeReset(12, 1);
1389 
1390  integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
1391  integer iFirstMomentumIndex = pNode->iGetFirstMomentumIndex();
1392  integer iFirstReactionIndex = iGetFirstIndex();
1393 
1394  /* Attenzione: modifico dividendo le equazioni di vincolo per dCoef */
1395  for (integer iCnt = 1; iCnt <= 6; iCnt++) {
1396  WM.PutItem(iCnt, iFirstReactionIndex + iCnt,
1397  iFirstPositionIndex + iCnt, 1.);
1398  WM.PutItem(6 + iCnt, iFirstMomentumIndex + iCnt,
1399  iFirstReactionIndex + iCnt, 1.);
1400  }
1401 
1402  /* Con l'aggiunta dei nodi statici non occorre piu' evitare la
1403  * singolarita' della matrice */
1404 
1405  return WorkMat;
1406 }
1407 
1408 /* Inverse Dynamics */
1411  const VectorHandler& /* XCurr */ )
1412 {
1413  DEBUGCOUT("Entering ClampJoint::AssJac()" << std::endl);
1414 
1415  SparseSubMatrixHandler& WM = WorkMat.SetSparse();
1416 
1417  /* Dimensiona e resetta la matrice di lavoro */
1418  WM.ResizeReset(6, 1);
1419 
1420  integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
1421  integer iFirstReactionIndex = iGetFirstIndex();
1422 
1423  for (integer iCnt = 1; iCnt <= 6; iCnt++) {
1424  WM.PutItem(iCnt, iFirstReactionIndex + iCnt,
1425  iFirstPositionIndex + iCnt, 1.);
1426  }
1427 
1428  return WorkMat;
1429 }
1430 
1431 /* assemblaggio matrici per autovalori */
1432 void
1434  VariableSubMatrixHandler& WorkMatB,
1435  const VectorHandler& XCurr,
1436  const VectorHandler& XPrimeCurr)
1437 {
1438  DEBUGCOUT("Entering ClampJoint::AssMats(); will result in call to AssJac()"
1439  << std::endl);
1440 
1441  WorkMatA.SetNullMatrix();
1442  AssJac(WorkMatB, 1., XCurr, XPrimeCurr);
1443 }
1444 
1445 
1448  doublereal dCoef,
1449  const VectorHandler& XCurr,
1450  const VectorHandler& /* XPrimeCurr */ )
1451 {
1452  DEBUGCOUT("Entering ClampJoint::AssRes()" << std::endl);
1453 
1454  WorkVec.ResizeReset(12);
1455 
1456  /* Indici delle incognite del nodo e delle reazioni */
1457  integer iFirstMomentumIndex = pNode->iGetFirstMomentumIndex();
1458  integer iFirstReactionIndex = iGetFirstIndex();
1459  for (integer iCnt = 1; iCnt <= 6; iCnt++) {
1460  WorkVec.PutRowIndex(iCnt, iFirstMomentumIndex + iCnt);
1461  WorkVec.PutRowIndex(6 + iCnt, iFirstReactionIndex + iCnt);
1462  }
1463 
1464  /* Aggiorna le reazioni vincolari */
1465  F = Vec3(XCurr, iFirstReactionIndex + 1);
1466  M = Vec3(XCurr, iFirstReactionIndex + 3 + 1);
1467 
1468  /* Calcola posizione e parametri di rotazione */
1469  const Vec3& x(pNode->GetXCurr());
1470  const Mat3x3& R(pNode->GetRCurr());
1471 
1472  Vec3 theta_c(RotManip::VecRot(R.MulMT(RClamp)));
1473 
1474  /* Residuo della riga di equilibrio */
1475  WorkVec.Sub(1, F);
1476  WorkVec.Sub(3 + 1, M);
1477 
1478  /* Modifica: divido le equazioni di vincolo per dCoef */
1479  ASSERT(dCoef != 0.);
1480  /* Residuo dell'equazione di vincolo */
1481  WorkVec.Sub(6 + 1, (x - XClamp)/dCoef);
1482  WorkVec.Sub(9 + 1, theta_c/dCoef);
1483 
1484  return WorkVec;
1485 }
1486 
1487 /* inverse dynamics capable element */
1488 bool
1490 {
1491  return true;
1492 }
1493 
1494 /* Inverse Dynamics: AssRes()*/
1497  const VectorHandler& XCurr,
1498  const VectorHandler& /* XPrimeCurr */,
1499  const VectorHandler& /* XPrimePrimeCurr */,
1500  InverseDynamics::Order iOrder)
1501 {
1502  DEBUGCOUT("Entering ClampJoint::AssRes()" << std::endl);
1503 
1504  /* The residual is != 0 only for position */
1505  if (iOrder == InverseDynamics::POSITION) {
1506  WorkVec.ResizeReset(6);
1507 
1508  /* FIXME: Indici delle incognite */
1509  integer iFirstReactionIndex = iGetFirstIndex();
1510  for (integer iCnt = 1; iCnt <= 6; iCnt++) {
1511  WorkVec.PutRowIndex(iCnt, iFirstReactionIndex+iCnt);
1512  }
1513 
1514  /* Calcola posizione e parametri di rotazione */
1515  const Vec3& x(pNode->GetXCurr());
1516  const Mat3x3& R(pNode->GetRCurr());
1517 
1518  Vec3 theta_c(RotManip::VecRot(R.MulMT(RClamp)));
1519 
1520  /* Residuo dell'equazione di vincolo */
1521  WorkVec.Sub(1, x - XClamp);
1522  WorkVec.Sub(3 + 1, theta_c);
1523 
1524  } else {
1525  WorkVec.Resize(0);
1526  }
1527 
1528  return WorkVec;
1529 }
1530 
1531 /* Inverse Dynamics update */
1532 void
1534 {
1536 
1537  integer iFirstReactionIndex = iGetFirstIndex();
1538 
1539  /* Aggiorna le reazioni vincolari */
1540  F = Vec3(XCurr, iFirstReactionIndex + 1);
1541  M = Vec3(XCurr, iFirstReactionIndex + 4);
1542 }
1543 
1544 void
1546 {
1547  if (bToBeOutput()) {
1548  const Mat3x3& R(pNode->GetRCurr());
1549 
1550  Joint::Output(OH.Joints(), "Clamp", GetLabel(),
1551  R.MulTV(F), R.MulTV(M), F, M) << std::endl;
1552  }
1553 }
1554 
1555 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
1558  const VectorHandler& /* XCurr */ )
1559 {
1560  DEBUGCOUT("Entering ClampJoint::InitialAssJac()" << std::endl);
1561 
1562  SparseSubMatrixHandler& WM = WorkMat.SetSparse();
1563  WM.ResizeReset(24, 1);
1564 
1565  integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
1566  integer iFirstVelocityIndex = iFirstPositionIndex+6;
1567  integer iFirstReactionIndex = iGetFirstIndex();
1568  integer iReactionPrimeIndex = iFirstReactionIndex+6;
1569 
1570  /*
1571  * x | 0 0 0 0 -I 0 0 0 || Delta_x | | f |
1572  * g | 0 0 0 0 0 -I 0 0 || Delta_g | | m |
1573  * xP | 0 0 0 0 0 0 -I 0 || Delta_xP | | fP |
1574  * w | 0 0 0 0 0 0 0 -I || Delta_w | | mP |
1575  * f | I 0 0 0 0 0 0 0 || Delta_f | = | X0-x |
1576  * m | 0 I 0 0 0 0 0 0 || Delta_m | | -xP |
1577  * fP | 0 0 I 0 0 0 0 0 || Delta_fP | | -g |
1578  * mP | 0 0 0 I 0 0 0 0 || Delta_mP | | -w |
1579  *
1580  *
1581  *
1582  */
1583 
1584  for (int iCnt = 1; iCnt <= 6; iCnt++) {
1585  WM.PutItem(iCnt, iFirstReactionIndex+iCnt,
1586  iFirstPositionIndex+iCnt, 1.);
1587 
1588  WM.PutItem(6+iCnt, iReactionPrimeIndex+iCnt,
1589  iFirstVelocityIndex+iCnt, 1.);
1590 
1591  WM.PutItem(12+iCnt, iFirstPositionIndex+iCnt,
1592  iFirstReactionIndex+iCnt, 1.);
1593 
1594  WM.PutItem(18+iCnt, iFirstVelocityIndex+iCnt,
1595  iReactionPrimeIndex+iCnt, 1.);
1596  }
1597 
1598  return WorkMat;
1599 }
1600 
1601 
1602 /* Contributo al residuo durante l'assemblaggio iniziale */
1605  const VectorHandler& XCurr)
1606 {
1607  DEBUGCOUT("Entering ClampJoint::InitialAssRes()" << std::endl);
1608 
1609  /* Dimensiona e resetta la matrice di lavoro */
1610  integer iNumRows = 0;
1611  integer iNumCols = 0;
1612  InitialWorkSpaceDim(&iNumRows, &iNumCols);
1613  WorkVec.ResizeReset(iNumRows);
1614 
1615  integer iFirstPositionIndex = pNode->iGetFirstPositionIndex();
1616  integer iFirstReactionIndex = iGetFirstIndex();
1617  integer iReactionPrimeIndex = iFirstReactionIndex+6;
1618 
1619  for (int iCnt = 1; iCnt <= 12; iCnt++) {
1620  WorkVec.PutRowIndex(iCnt, iFirstPositionIndex+iCnt);
1621  WorkVec.PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
1622  }
1623 
1624 
1625  /* Forza */
1626  WorkVec.Put(1, -Vec3(XCurr, iFirstReactionIndex+1));
1627 
1628  /* Coppia */
1629  WorkVec.Put(4, -Vec3(XCurr, iFirstReactionIndex+4));
1630 
1631  /* Derivata della Forza */
1632  WorkVec.Put(7, -Vec3(XCurr, iReactionPrimeIndex+1));
1633 
1634  /* Derivata della Coppia */
1635  WorkVec.Put(10, -Vec3(XCurr, iReactionPrimeIndex+4));
1636 
1637  /* Posizione */
1638  WorkVec.Put(13, XClamp-pNode->GetXCurr());
1639 
1640  /* Parametri di rotazione */
1641  Mat3x3 R(pNode->GetRCurr());
1642  WorkVec.Put(16, -Vec3(CGR_Rot::Param, R.MulMT(RClamp)));
1643 
1644  /* Velocita' */
1645  WorkVec.Put(19, -pNode->GetVCurr());
1646 
1647  /* Velocita' angolare */
1648  WorkVec.Put(22, -pNode->GetWCurr());
1649 
1650  return WorkVec;
1651 }
1652 
1653 void
1655  VectorHandler& X, VectorHandler& XP,
1657 {
1658  integer iFirstReactionIndex = iGetFirstIndex();
1659  X.Put(iFirstReactionIndex+1,F);
1660  X.Put(iFirstReactionIndex+4,M);
1661 }
1662 
1663 /* Metodi per l'estrazione di dati "privati".
1664  * Si suppone che l'estrattore li sappia interpretare.
1665  * Come default non ci sono dati privati estraibili */
1666 unsigned int
1668 {
1669  return 6;
1670 }
1671 
1672 unsigned int
1673 ClampJoint::iGetPrivDataIdx(const char *s) const
1674 {
1675  ASSERT(s != NULL);
1676 
1677  unsigned int idx = 0;
1678 
1679  switch (s[0]) {
1680  case 'F':
1681  break;
1682 
1683  case 'M':
1684  idx += 3;
1685  break;
1686 
1687  default:
1688  return 0;
1689  }
1690 
1691  switch (s[1]) {
1692  case 'x':
1693  idx += 1;
1694  break;
1695 
1696  case 'y':
1697  idx += 2;
1698  break;
1699 
1700  case 'z':
1701  idx += 3;
1702  break;
1703 
1704  default:
1705  return 0;
1706  }
1707 
1708  if (s[2] != '\0') {
1709  return 0;
1710  }
1711 
1712  return idx;
1713 }
1714 
1715 doublereal
1716 ClampJoint::dGetPrivData(unsigned int i) const
1717 {
1718  if (i >= 1 && i <= 3) {
1719  return F(i);
1720  }
1721 
1722  if (i >= 4 && i <= 6) {
1723  return M(i - 3);
1724  }
1725 
1727 }
1728 
1729 /* ClampJoint - end */
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: genj.cc:111
void AssMats(VariableSubMatrixHandler &WorkMatA, VariableSubMatrixHandler &WorkMatB, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: genj.cc:1433
Mat3x3 RClamp
Definition: genj.h:246
void PutColIndex(integer iSubCol, integer iCol)
Definition: submat.h:325
virtual void Output(OutputHandler &OH) const
Definition: genj.cc:268
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: genj.cc:1604
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: genj.cc:1046
void PutMat3x3(integer iSubIt, integer iFirstRow, integer iFirstCol, const Mat3x3 &m)
Definition: submat.cc:1331
const Vec3 Zero3(0., 0., 0.)
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
const Param_Manip Param
Definition: matvec3.cc:644
std::ostream & Write(std::ostream &out, const char *sFill=" ") const
Definition: matvec3.cc:738
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: genj.cc:796
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: genj.cc:202
long int flag
Definition: mbdyn.h:43
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: genj.cc:1557
virtual const Mat3x3 & GetRRef(void) const
Definition: strnode.h:1006
virtual bool bToBeOutput(void) const
Definition: output.cc:890
virtual std::ostream & Restart(std::ostream &out) const
Definition: genj.cc:669
GradientExpression< BinaryExpr< FuncPow, LhsExpr, RhsExpr > > pow(const GradientExpression< LhsExpr > &u, const GradientExpression< RhsExpr > &v)
Definition: gradient.h:2961
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
Definition: matvec3.h:98
virtual doublereal dGetPrivData(unsigned int i) const
Definition: genj.cc:88
virtual void ResizeReset(integer)
Definition: vh.cc:55
virtual unsigned int iGetPrivDataIdx(const char *s) const
Definition: genj.cc:76
const MatCross_Manip MatCross
Definition: matvec3.cc:639
FullSubMatrixHandler & SetFull(void)
Definition: submat.h:1168
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
virtual Node::Type GetNodeType(void) const
Definition: strnode.cc:145
virtual std::ostream & Restart(std::ostream &out) const
Definition: genj.cc:100
doublereal Dot(const Vec3 &v) const
Definition: matvec3.h:243
void Add(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:209
const StructNode * pNode
Definition: genj.h:244
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: genj.cc:547
~DistanceJoint(void)
Definition: genj.cc:62
virtual void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: genj.h:81
virtual void Sub(integer iRow, const Vec3 &v)
Definition: vh.cc:78
doublereal dGet(void) const
Definition: drive.cc:671
void PutCoef(integer iRow, integer iCol, const doublereal &dCoef)
Definition: submat.h:672
void ResizeReset(integer iNewRow, integer iNewCol)
Definition: submat.cc:1084
virtual std::ostream & DescribeDof(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: genj.cc:1231
virtual const Vec3 & GetWRef(void) const
Definition: strnode.h:1024
#define NO_OP
Definition: myassert.h:74
void PutCross(integer iSubIt, integer iFirstRow, integer iFirstCol, const Vec3 &v)
Definition: submat.cc:1236
doublereal dAlpha
Definition: genj.h:49
std::vector< Hint * > Hints
Definition: simentity.h:89
Vec3 XClamp
Definition: genj.h:245
virtual unsigned int iGetPrivDataIdx(const char *s) const
Definition: genj.cc:1673
GradientExpression< UnaryExpr< FuncFabs, Expr > > fabs(const GradientExpression< Expr > &u)
Definition: gradient.h:2973
Vec3 F
Definition: genj.h:247
virtual std::ostream & Restart(std::ostream &out) const =0
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
virtual void SetInitialValue(VectorHandler &X)
Definition: genj.cc:1116
virtual unsigned int iGetPrivDataIdx(const char *s) const
Definition: genj.cc:644
Vec3 MulTV(const Vec3 &v) const
Definition: matvec3.cc:482
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: genj.cc:1378
const StructNode * pNode1
Definition: genj.h:142
static const char * dof[]
Definition: genj.cc:1254
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
void Update(const VectorHandler &XCurr, InverseDynamics::Order iOrder=InverseDynamics::INVERSE_DYNAMICS)
Definition: genj.cc:1533
void PutItem(integer iSubIt, integer iRow, integer iCol, const doublereal &dCoef)
Definition: submat.h:997
DistanceJointWithOffset(unsigned int uL, const DofOwner *pDO, const StructNode *pN1, const StructNode *pN2, const Vec3 &f1Tmp, const Vec3 &f2Tmp, const DriveCaller *pDC, flag fOut)
Definition: genj.cc:608
const StructDispNode * pNode1
Definition: genj.h:46
void SetNullMatrix(void)
Definition: submat.h:1159
const StructNode * pNode2
Definition: genj.h:143
const doublereal & dGet(unsigned short int iRow) const
Definition: matvec3.h:285
SubVectorHandler & InitialAssRes(SubVectorHandler &WorkVec, const VectorHandler &XCurr)
Definition: genj.cc:447
virtual bool bInverseDynamics(void) const
Definition: genj.cc:1489
ClampJoint(unsigned int uL, const DofOwner *pD, const StructNode *pN, const Vec3 &X0, const Mat3x3 &R0, flag fOut)
Definition: genj.cc:1212
#define DEBUGCOUT(msg)
Definition: myassert.h:232
Vec3 v
Definition: genj.h:48
virtual ~ClampJoint(void)
Definition: genj.cc:1224
static const char xyz[]
Definition: genj.cc:1253
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: genj.cc:1151
virtual integer iGetFirstMomentumIndex(void) const =0
virtual integer iGetFirstPositionIndex(void) const
Definition: strnode.h:452
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: genj.cc:878
unsigned int uLabel
Definition: withlab.h:44
virtual std::ostream & Restart(std::ostream &out) const
Definition: joint.h:195
virtual unsigned int iGetNumPrivData(void) const
Definition: genj.cc:638
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: genj.cc:1447
std::ostream & Joints(void) const
Definition: output.h:443
DriveCaller * pCreateDrive(DataManager *pDM) const
Definition: hint_impl.cc:117
DotTraits< VectorExprLhs, VectorExprRhs, N_rows, N_rows >::ExpressionType Dot(const VectorExpression< VectorExprLhs, N_rows > &u, const VectorExpression< VectorExprRhs, N_rows > &v)
Definition: matvec.h:3133
~DistanceJointWithOffset(void)
Definition: genj.cc:631
#define ASSERT(expression)
Definition: colamd.c:977
DistanceJoint(unsigned int uL, const DofOwner *pDO, const StructDispNode *pN1, const StructDispNode *pN2, const DriveCaller *pDC, flag fOut)
Definition: genj.cc:49
virtual unsigned int iGetNumPrivData(void) const
Definition: genj.cc:1667
GradientExpression< UnaryExpr< FuncSqrt, Expr > > sqrt(const GradientExpression< Expr > &u)
Definition: gradient.h:2974
virtual void PutCoef(integer iRow, const doublereal &dCoef)=0
VectorExpression< VectorCrossExpr< VectorLhsExpr, VectorRhsExpr >, 3 > Cross(const VectorExpression< VectorLhsExpr, 3 > &u, const VectorExpression< VectorRhsExpr, 3 > &v)
Definition: matvec.h:3248
Vec3 M
Definition: genj.h:248
virtual void InitialWorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: genj.h:339
DriveCaller * pGetDriveCaller(void) const
Definition: drive.cc:658
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
virtual void Add(integer iRow, const Vec3 &v)
Definition: vh.cc:63
virtual void ResizeReset(integer, integer)
Definition: submat.cc:182
virtual doublereal dGet(const doublereal &dVar) const =0
const MatCrossCross_Manip MatCrossCross
Definition: matvec3.cc:640
virtual void Put(integer iRow, const Vec3 &v)
Definition: vh.cc:93
virtual std::ostream & DescribeEq(std::ostream &out, const char *prefix="", bool bInitial=false) const
Definition: genj.cc:1302
const StructDispNode * pNode2
Definition: genj.h:47
VariableSubMatrixHandler & InitialAssJac(VariableSubMatrixHandler &WorkMat, const VectorHandler &XCurr)
Definition: genj.cc:290
Definition: elem.h:75
virtual void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: genj.h:181
virtual void Output(OutputHandler &OH) const
Definition: genj.cc:1545
void PutRowIndex(integer iSubRow, integer iRow)
Definition: submat.h:311
void Set(const DriveCaller *pDC)
Definition: drive.cc:647
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
void Sub(integer iRow, integer iCol, const Vec3 &v)
Definition: submat.cc:215
doublereal dAlpha
Definition: genj.h:147
virtual void SetValue(DataManager *pDM, VectorHandler &X, VectorHandler &XP, SimulationEntity::Hints *ph=0)
Definition: genj.cc:1654
Definition: joint.h:50
Mat3x3 MulMT(const Mat3x3 &m) const
Definition: matvec3.cc:444
void ReadInitialState(MBDynParser &HP)
Definition: genj.cc:1360
SparseSubMatrixHandler & SetSparse(void)
Definition: submat.h:1178
virtual integer iGetFirstIndex(void) const
Definition: dofown.h:127
double doublereal
Definition: colamd.c:52
virtual void Output(OutputHandler &OH) const
Definition: genj.cc:859
virtual std::ostream & Restart(std::ostream &out) const
Definition: genj.cc:1368
std::ostream & Output(std::ostream &out, const char *sJointName, unsigned int uLabel, const Vec3 &FLocal, const Vec3 &MLocal, const Vec3 &FGlobal, const Vec3 &MGlobal) const
Definition: joint.cc:138
long int integer
Definition: colamd.c:51
unsigned int GetLabel(void) const
Definition: withlab.cc:62
VariableSubMatrixHandler & AssJac(VariableSubMatrixHandler &WorkMat, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: genj.cc:684
virtual Vec3 GetVec3(void)
Definition: mbpar.cc:2220
virtual void SetInitialValue(VectorHandler &X)
Definition: genj.cc:521
static const char * eq[]
Definition: genj.cc:1260
virtual doublereal dGetPrivData(unsigned int i) const
Definition: genj.cc:656
virtual void Resize(integer iNewSize)=0
Mat3x3 R
virtual unsigned int iGetNumPrivData(void) const
Definition: genj.cc:70
virtual doublereal dGetPrivData(unsigned int i) const
Definition: genj.cc:1716