MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
strext.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/strext.cc,v 1.68 2017/07/31 14:19:54 masarati Exp $ */
2 /*
3  * MBDyn (C) is a multibody analysis code.
4  * http://www.mbdyn.org
5  *
6  * Copyright (C) 2007-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 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
33 
34 #include "dataman.h"
35 #include "extedge.h"
36 #include "strext.h"
37 #include "stredge.h"
38 #include "Rot.hh"
39 
40 #include <fstream>
41 #include <cerrno>
42 #include <algorithm>
43 
44 /* StructExtForce - begin */
45 
46 /* Costruttore */
47 StructExtForce::StructExtForce(unsigned int uL,
48  DataManager *pDM,
49  const StructNode *pRefNode,
50  bool bUseReferenceNodeForces,
51  bool bRotateReferenceNodeForces,
52  std::vector<unsigned >& labels,
53  std::vector<const StructNode *>& nodes,
54  std::vector<Vec3>& offsets,
55  bool bSorted,
56  bool bLabels,
57  bool bOutputAccelerations,
58  unsigned uRot,
59  ExtFileHandlerBase *pEFH,
60  bool bSendAfterPredict,
61  int iCoupling,
62  unsigned uOutputFlags,
63  flag fOut)
64 : Elem(uL, fOut),
65 ExtForce(uL, pDM, pEFH, bSendAfterPredict, iCoupling, fOut),
66 pRefNode(pRefNode),
67 bUseReferenceNodeForces(bUseReferenceNodeForces),
68 bRotateReferenceNodeForces(bRotateReferenceNodeForces),
69 F0(Zero3), M0(Zero3),
70 F1(Zero3), M1(Zero3),
71 F2(Zero3), M2(Zero3),
72 uOutputFlags(uOutputFlags),
73 bLabels(bLabels),
74 bSorted(bSorted),
75 uRot(uRot),
76 bOutputAccelerations(bOutputAccelerations),
77 iobuf_labels(0),
78 iobuf_x(0),
79 iobuf_R(0),
80 iobuf_theta(0),
81 iobuf_euler_123(0),
82 iobuf_xp(0),
83 iobuf_omega(0),
84 iobuf_xpp(0),
85 iobuf_omegap(0),
86 iobuf_f(0),
87 iobuf_m(0)
88 {
89  ASSERT(nodes.size() == offsets.size());
90  ASSERT((!bLabels) || (nodes.size() == offsets.size()));
91 
92  m_Points.resize(nodes.size());
93 
94  switch (uRot) {
95  case MBC_ROT_NONE:
96  case MBC_ROT_THETA:
97  case MBC_ROT_MAT:
98  case MBC_ROT_EULER_123:
99  break;
100 
101  default:
102  silent_cerr("StructExtForce(" << GetLabel() << "): "
103  "unknown rotation format " << uRot
104  << std::endl);
105  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
106  }
107 
108  for (unsigned i = 0; i < nodes.size(); i++) {
109  m_Points[i].pNode = nodes[i];
110  m_Points[i].Offset = offsets[i];
111  m_Points[i].F = Zero3;
112  m_Points[i].M = Zero3;
113  if (bLabels) {
114  m_Points[i].uLabel = labels[i];
115  } else {
116  m_Points[i].uLabel = m_Points[i].pNode->GetLabel();
117  }
118  }
119 
120  ASSERT(!(!bLabels && !bSorted));
121  if (!bSorted) {
122  done.resize(nodes.size());
123  }
124 
125  if (bOutputAccelerations) {
126  for (unsigned i = 0; i < m_Points.size(); i++) {
127  const DynamicStructNode *pDSN = dynamic_cast<const DynamicStructNode *>(m_Points[i].pNode);
128  if (pDSN == 0) {
129  silent_cerr("StructExtForce"
130  "(" << GetLabel() << "): "
131  "point #" << i << " StructNode(" << m_Points[i].pNode->GetLabel() << ") "
132  "is not dynamic"
133  << std::endl);
134  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
135  }
136 
137  const_cast<DynamicStructNode *>(pDSN)->ComputeAccelerations(true);
138  }
139  }
140 
141  unsigned node_kinematics_size = 0;
142  unsigned labels_size = 0;
143 
144  // I/O will use filedes
145  if (!pEFH->GetOutStream()) {
146  node_kinematics_size = 3 + 3;
147 
148  switch (uRot) {
149  case MBC_ROT_NONE:
150  break;
151 
152  case MBC_ROT_MAT:
153  node_kinematics_size += 9 + 3;
154  break;
155 
156  case MBC_ROT_THETA:
157  case MBC_ROT_EULER_123:
158  node_kinematics_size += 3 + 3;
159  break;
160 
161  default:
162  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
163  }
164 
165  if (bOutputAccelerations) {
166  node_kinematics_size += 3;
167  if (uRot != MBC_ROT_NONE) {
168  node_kinematics_size += 3;
169  }
170  }
171 
172  node_kinematics_size *= sizeof(doublereal);
173 
174  dynamics_size = 3;
175  if (uRot != MBC_ROT_NONE) {
176  dynamics_size += 3;
177  }
178 
179  dynamics_size *= sizeof(doublereal);
180 
181  if (bLabels) {
182  labels_size = sizeof(uint32_t)*(m_Points.size() + m_Points.size()%2);
183  }
184 
185  iobuf.resize(node_kinematics_size*m_Points.size() + labels_size);
186  dynamics_size = dynamics_size*m_Points.size() + labels_size;
187 
188  char *ptr = &iobuf[0];
189  if (bLabels) {
190  iobuf_labels = (uint32_t *)ptr;
191  ptr += labels_size;
192  }
193 
194  iobuf_x = (doublereal *)ptr;
195  ptr += 3*sizeof(doublereal)*m_Points.size();
196 
197  switch (uRot) {
198  case MBC_ROT_NONE:
199  break;
200 
201  case MBC_ROT_MAT:
202  iobuf_R = (doublereal *)ptr;
203  ptr += 9*sizeof(doublereal)*m_Points.size();
204  break;
205 
206  case MBC_ROT_THETA:
207  iobuf_theta = (doublereal *)ptr;
208  ptr += 3*sizeof(doublereal)*m_Points.size();
209  break;
210 
211  case MBC_ROT_EULER_123:
212  iobuf_euler_123 = (doublereal *)ptr;
213  ptr += 3*sizeof(doublereal)*m_Points.size();
214  break;
215  }
216 
217  iobuf_xp = (doublereal *)ptr;
218  ptr += 3*sizeof(doublereal)*m_Points.size();
219 
220  if (uRot != MBC_ROT_NONE) {
221  iobuf_omega = (doublereal *)ptr;
222  ptr += 3*sizeof(doublereal)*m_Points.size();
223  }
224 
225  if (bOutputAccelerations) {
226  iobuf_xpp = (doublereal *)ptr;
227  ptr += 3*sizeof(doublereal)*m_Points.size();
228 
229  if (uRot != MBC_ROT_NONE) {
230  iobuf_omegap = (doublereal *)ptr;
231  ptr += 3*sizeof(doublereal)*m_Points.size();
232  }
233  }
234 
235  ptr = (char *)iobuf_x;
236 
237  iobuf_f = (doublereal *)ptr;
238  ptr += 3*sizeof(doublereal)*m_Points.size();
239 
240  if (uRot != MBC_ROT_NONE) {
241  iobuf_m = (doublereal *)ptr;
242  ptr += 3*sizeof(doublereal)*m_Points.size();
243  }
244  }
245 }
246 
247 StructExtForce::~StructExtForce(void)
248 {
249  NO_OP;
250 }
251 
252 void
253 StructExtForce::WorkSpaceDim(integer* piNumRows, integer* piNumCols) const
254 {
255  if (iCoupling == COUPLING_NONE) {
256  *piNumRows = 0;
257  *piNumCols = 0;
258 
259  } else {
260  *piNumRows = (pRefNode ? 6 : 0) + 6*m_Points.size();
261  *piNumCols = 1;
262  }
263 }
264 
265 bool
266 StructExtForce::Prepare(ExtFileHandlerBase *pEFH)
267 {
268  bool bResult = true;
269 
270  switch (pEFH->NegotiateRequest()) {
271  case ExtFileHandlerBase::NEGOTIATE_NO:
272  break;
273 
274  case ExtFileHandlerBase::NEGOTIATE_CLIENT: {
275  std::ostream *outfp = pEFH->GetOutStream();
276  if (outfp) {
277 
278 #ifdef USE_SOCKET
279  } else {
280  char buf[sizeof(uint32_t) + sizeof(uint32_t)];
281  uint32_t *uint32_ptr;
282 
283  uint32_ptr = (uint32_t *)&buf[0];
284  uint32_ptr[0] = MBC_NODAL | uRot;
285  if (pRefNode != 0) {
286  uint32_ptr[0] |= MBC_REF_NODE;
287  }
288 
289  if (bLabels) {
290  uint32_ptr[0] |= MBC_LABELS;
291  }
292 
293  if (bOutputAccelerations) {
294  uint32_ptr[0] |= MBC_ACCELS;
295  }
296 
297  uint32_ptr[1] = m_Points.size();
298 
299  ssize_t rc = send(pEFH->GetOutFileDes(),
300  (const void *)buf, sizeof(buf),
301  pEFH->GetSendFlags());
302  if (rc == -1) {
303  int save_errno = errno;
304  char *err_msg = strerror(save_errno);
305  silent_cerr("StructExtForce(" << GetLabel() << "): "
306  "negotiation request send() failed "
307  "(" << save_errno << ": " << err_msg << ")"
308  << std::endl);
309  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
310 
311  } else if (rc != sizeof(buf)) {
312  silent_cerr("StructExtForce(" << GetLabel() << "): "
313  "negotiation request send() failed "
314  "(sent " << rc << " of " << sizeof(buf) << " bytes)"
315  << std::endl);
316  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
317  }
318 #endif // USE_SOCKET
319  }
320  } break;
321 
322  case ExtFileHandlerBase::NEGOTIATE_SERVER: {
323  unsigned uN;
324  unsigned uNodal;
325  bool bRef;
326  unsigned uR;
327  bool bA;
328  bool bL;
329 
330  std::istream *infp = pEFH->GetInStream();
331  if (infp) {
332  // TODO: stream negotiation?
333 
334 #ifdef USE_SOCKET
335  } else {
336  char buf[sizeof(uint32_t) + sizeof(uint32_t)];
337  uint32_t *uint32_ptr;
338 
339  ssize_t rc = recv(pEFH->GetInFileDes(),
340  (void *)buf, sizeof(buf),
341  pEFH->GetRecvFlags());
342  if (rc == -1) {
343  int save_errno = errno;
344  char *err_msg = strerror(save_errno);
345  silent_cerr("StructExtForce(" << GetLabel() << "): "
346  "negotiation response recv() failed "
347  "(" << save_errno << ": " << err_msg << ")"
348  << std::endl);
349  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
350 
351  } else if (rc != sizeof(buf)) {
352  silent_cerr("StructExtForce(" << GetLabel() << "): "
353  "negotiation response recv() failed "
354  "(got " << rc << " of " << sizeof(buf) << " bytes)"
355  << std::endl);
356  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
357  }
358 
359  uint32_ptr = (uint32_t *)&buf[0];
360  uNodal = (uint32_ptr[0] & MBC_MODAL_NODAL_MASK);
361  bRef = (uint32_ptr[0] & MBC_REF_NODE);
362  uR = (uint32_ptr[0] & MBC_ROT_MASK);
363  bL = (uint32_ptr[0] & MBC_LABELS);
364  bA = (uint32_ptr[0] & MBC_ACCELS);
365 
366  uN = uint32_ptr[1];
367 #endif // USE_SOCKET
368  }
369 
370  if (uNodal != MBC_NODAL) {
371  silent_cerr("StructExtForce(" << GetLabel() << "): "
372  "negotiation response failed: expecting MBC_NODAL "
373  "(=" << MBC_NODAL << "), got " << uNodal
374  << std::endl);
375  bResult = false;
376  }
377 
378  if ((pRefNode != 0 && !bRef) || (pRefNode == 0 && bRef)) {
379  silent_cerr("StructExtForce(" << GetLabel() << "): "
380  "negotiation response failed: reference node configuration mismatch "
381  "(local=" << (pRefNode != 0 ? "yes" : "no") << ", remote=" << (bRef ? "yes" : "no") << ")"
382  << std::endl);
383  bResult = false;
384  }
385 
386  if (uR != uRot) {
387  silent_cerr("StructExtForce(" << GetLabel() << "): "
388  "negotiation response failed: orientation output mismatch "
389  "(local=" << uRot << ", remote=" << uR << ")"
390  << std::endl);
391  bResult = false;
392  }
393 
394  if (bL != bLabels) {
395  silent_cerr("StructExtForce(" << GetLabel() << "): "
396  "negotiation response failed: labels output mismatch "
397  "(local=" << (bLabels ? "yes" : "no") << ", remote=" << (bL ? "yes" : "no") << ")"
398  << std::endl);
399  bResult = false;
400  }
401 
402  if (bA != bOutputAccelerations) {
403  silent_cerr("StructExtForce(" << GetLabel() << "): "
404  "negotiation response failed: acceleration output mismatch "
405  "(local=" << (bOutputAccelerations ? "yes" : "no") << ", remote=" << (bA ? "yes" : "no") << ")"
406  << std::endl);
407  bResult = false;
408  }
409 
410  if (uN != m_Points.size()) {
411  silent_cerr("StructExtForce(" << GetLabel() << "): "
412  "negotiation response failed: node number mismatch "
413  "(local=" << m_Points.size() << ", remote=" << uN << ")"
414  << std::endl);
415  bResult = false;
416  }
417  } break;
418 
419  default:
420  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
421  }
422 
423  return bResult;
424 }
425 
426 /*
427  * Send output to companion software
428  */
429 void
430 StructExtForce::Send(ExtFileHandlerBase *pEFH, ExtFileHandlerBase::SendWhen when)
431 {
432  std::ostream *outfp = pEFH->GetOutStream();
433  if (outfp) {
434  SendToStream(*outfp, when);
435 
436  } else {
437  SendToFileDes(pEFH->GetOutFileDes(), when);
438  }
439 }
440 
441 void
442 StructExtForce::SendToStream(std::ostream& outf, ExtFileHandlerBase::SendWhen when)
443 {
444  if (pRefNode) {
445  const Vec3& xRef = pRefNode->GetXCurr();
446  const Mat3x3& RRef = pRefNode->GetRCurr();
447  const Vec3& xpRef = pRefNode->GetVCurr();
448  const Vec3& wRef = pRefNode->GetWCurr();
449  const Vec3& xppRef = pRefNode->GetXPPCurr();
450  const Vec3& wpRef = pRefNode->GetWPCurr();
451 
452  outf << "# reference node" << std::endl;
453 
454  if (bLabels) {
455  outf
456  << pRefNode->GetLabel()
457  << " ";
458  }
459 
460  outf
461  << xRef;
462 
463  switch (uRot) {
464  case MBC_ROT_NONE:
465  break;
466 
467  case MBC_ROT_MAT:
468  outf
469  << " " << RRef;
470  break;
471 
472  case MBC_ROT_THETA:
473  outf
474  << " " << RotManip::VecRot(RRef);
475  break;
476 
477  case MBC_ROT_EULER_123:
478  outf
479  << " " << MatR2EulerAngles123(RRef)*dRaDegr;
480  break;
481  }
482 
483  outf
484  << " " << xpRef;
485 
486  if (uRot != MBC_ROT_NONE) {
487  outf
488  << " " << wRef;
489  }
490 
491  if (bOutputAccelerations) {
492  outf
493  << " " << xppRef;
494 
495  if (uRot != MBC_ROT_NONE) {
496  outf
497  << " " << wpRef;
498  }
499  }
500  outf << std::endl;
501 
502  outf << "# regular nodes" << std::endl;
503 
504  for (std::vector<PointData>::const_iterator point = m_Points.begin(); point != m_Points.end(); ++point) {
505  Vec3 f(point->pNode->GetRCurr()*point->Offset);
506  Vec3 x(point->pNode->GetXCurr() + f);
507  Vec3 Dx(x - xRef);
508  Mat3x3 DR(RRef.MulTM(point->pNode->GetRCurr()));
509  Vec3 v(point->pNode->GetVCurr() + point->pNode->GetWCurr().Cross(f));
510  Vec3 Dv(v - xpRef - wRef.Cross(Dx));
511  const Vec3& w(point->pNode->GetWCurr());
512 
513  // manipulate
514 
515  if (bLabels) {
516  outf
517  << point->pNode->GetLabel()
518  << " ";
519  }
520 
521  outf
522  << RRef.MulTV(Dx);
523 
524  switch (uRot) {
525  case MBC_ROT_NONE:
526  break;
527 
528  case MBC_ROT_MAT:
529  outf
530  << " " << DR;
531  break;
532 
533  case MBC_ROT_THETA:
534  outf
535  << " " << RotManip::VecRot(DR);
536  break;
537 
538  case MBC_ROT_EULER_123:
539  outf
540  << " " << MatR2EulerAngles123(DR)*dRaDegr;
541  break;
542  }
543 
544  outf
545  << " " << RRef.MulTV(Dv);
546 
547  if (uRot != MBC_ROT_NONE) {
548  outf
549  << " " << RRef.MulTV(w - wRef);
550  }
551 
552  if (bOutputAccelerations) {
553  const Vec3& xpp(point->pNode->GetXPPCurr());
554  const Vec3& wp(point->pNode->GetWPCurr());
555 
556  outf
557  << " " << RRef.MulTV(xpp - xppRef - wpRef.Cross(Dx)
558  - wRef.Cross(wRef.Cross(Dx) + Dv*2)
559  + wp.Cross(f) + w.Cross(w.Cross(f)));
560  if (uRot != MBC_ROT_NONE) {
561  outf
562  << " " << RRef.MulTV(wp - wpRef - wRef.Cross(w));
563  }
564  }
565  outf << std::endl;
566  }
567 
568  } else {
569  outf << "# regular nodes" << std::endl;
570 
571  for (std::vector<PointData>::const_iterator point = m_Points.begin(); point != m_Points.end(); ++point) {
572  /*
573  p = x + f
574  R = R
575  v = xp + w cross f
576  w = w
577  a = xpp + wp cross f + w cross w cross f
578  wp = wp
579  */
580 
581  // Optimization of the above formulas
582  const Mat3x3& R = point->pNode->GetRCurr();
583  Vec3 f = R*point->Offset;
584  Vec3 x = point->pNode->GetXCurr() + f;
585  const Vec3& w = point->pNode->GetWCurr();
586  Vec3 wCrossf = w.Cross(f);
587  Vec3 v = point->pNode->GetVCurr() + wCrossf;
588 
589  if (bLabels) {
590  outf
591  << point->pNode->GetLabel()
592  << " ";
593  }
594 
595  outf
596  << x;
597 
598  switch (uRot) {
599  case MBC_ROT_NONE:
600  break;
601 
602  case MBC_ROT_MAT:
603  outf
604  << " " << R;
605  break;
606 
607  case MBC_ROT_THETA:
608  outf
609  << " " << RotManip::VecRot(R);
610  break;
611 
612  case MBC_ROT_EULER_123:
613  outf
614  << " " << MatR2EulerAngles123(R)*dRaDegr;
615  break;
616  }
617  outf
618  << " " << v;
619 
620  if (uRot != MBC_ROT_NONE) {
621  outf
622  << " " << w;
623  }
624 
625  if (bOutputAccelerations) {
626  const Vec3& wp = point->pNode->GetWPCurr();
627  Vec3 a = point->pNode->GetXPPCurr() + wp.Cross(f) + w.Cross(wCrossf);
628 
629  outf
630  << " " << a;
631 
632  if (uRot != MBC_ROT_NONE) {
633  outf
634  << " " << wp;
635  }
636  }
637 
638  outf << std::endl;
639  }
640  }
641 }
642 
643 void
644 StructExtForce::SendToFileDes(int outfd, ExtFileHandlerBase::SendWhen when)
645 {
646 #ifdef USE_SOCKET
647  if (pRefNode) {
648  const Vec3& xRef = pRefNode->GetXCurr();
649  const Mat3x3& RRef = pRefNode->GetRCurr();
650  const Vec3& xpRef = pRefNode->GetVCurr();
651  const Vec3& wRef = pRefNode->GetWCurr();
652  const Vec3& xppRef = pRefNode->GetXPPCurr();
653  const Vec3& wpRef = pRefNode->GetWPCurr();
654 
655  if (bLabels) {
656  uint32_t l[2];
657  l[0] = pRefNode->GetLabel();
658  l[1] = 0;
659  send(outfd, (void *)&l[0], sizeof(l), 0);
660  }
661 
662  send(outfd, (void *)xRef.pGetVec(), 3*sizeof(doublereal), 0);
663  switch (uRot) {
664  case MBC_ROT_NONE:
665  break;
666 
667  case MBC_ROT_MAT:
668  send(outfd, (void *)RRef.pGetMat(), 9*sizeof(doublereal), 0);
669  break;
670 
671  case MBC_ROT_THETA: {
672  Vec3 Theta(RotManip::VecRot(RRef));
673  send(outfd, (void *)Theta.pGetVec(), 3*sizeof(doublereal), 0);
674  } break;
675 
676  case MBC_ROT_EULER_123: {
677  Vec3 E(MatR2EulerAngles123(RRef)*dRaDegr);
678  send(outfd, (void *)E.pGetVec(), 3*sizeof(doublereal), 0);
679  } break;
680  }
681  send(outfd, (void *)xpRef.pGetVec(), 3*sizeof(doublereal), 0);
682  if (uRot != MBC_ROT_NONE) {
683  send(outfd, (void *)wRef.pGetVec(), 3*sizeof(doublereal), 0);
684  }
685  if (bOutputAccelerations) {
686  send(outfd, (void *)xppRef.pGetVec(), 3*sizeof(doublereal), 0);
687  if (uRot != MBC_ROT_NONE) {
688  send(outfd, (void *)wpRef.pGetVec(), 3*sizeof(doublereal), 0);
689  }
690  }
691 
692  for (unsigned i = 0; i < m_Points.size(); i++) {
693  const PointData& point = m_Points[i];
694  Vec3 f(point.pNode->GetRCurr()*point.Offset);
695  Vec3 x(point.pNode->GetXCurr() + f);
696  Vec3 Dx(x - xRef);
697  Mat3x3 DR(RRef.MulTM(point.pNode->GetRCurr()));
698  Vec3 v(point.pNode->GetVCurr() + point.pNode->GetWCurr().Cross(f));
699  Vec3 Dv(v - xpRef - wRef.Cross(Dx));
700  const Vec3& w(point.pNode->GetWCurr());
701 
702  // manipulate
703  if (bLabels) {
704  uint32_t l = point.pNode->GetLabel();
705  iobuf_labels[i] = l;
706  }
707 
708  Vec3 xTilde(RRef.MulTV(Dx));
709  memcpy(&iobuf_x[3*i], xTilde.pGetVec(), 3*sizeof(doublereal));
710 
711  switch (uRot) {
712  case MBC_ROT_NONE:
713  break;
714 
715  case MBC_ROT_MAT:
716  memcpy(&iobuf_R[9*i], DR.pGetMat(), 9*sizeof(doublereal));
717  break;
718 
719  case MBC_ROT_THETA: {
720  Vec3 Theta(RotManip::VecRot(DR));
721  memcpy(&iobuf_theta[3*i], Theta.pGetVec(), 3*sizeof(doublereal));
722  } break;
723 
724  case MBC_ROT_EULER_123: {
725  Vec3 E(MatR2EulerAngles123(DR)*dRaDegr);
726  memcpy(&iobuf_euler_123[3*i], E.pGetVec(), 3*sizeof(doublereal));
727  } break;
728  }
729 
730  Vec3 vTilde(RRef.MulTV(Dv));
731  memcpy(&iobuf_xp[3*i], vTilde.pGetVec(), 3*sizeof(doublereal));
732 
733  if (uRot != MBC_ROT_NONE) {
734  Vec3 wTilde(RRef.MulTV(w - wRef));
735  memcpy(&iobuf_omega[3*i], wTilde.pGetVec(), 3*sizeof(doublereal));
736  }
737 
738  if (bOutputAccelerations) {
739  const Vec3& xpp = point.pNode->GetXPPCurr();
740  const Vec3& wp = point.pNode->GetWPCurr();
741 
742  Vec3 xppTilde(RRef.MulTV(xpp - xppRef - wpRef.Cross(Dx)
743  - wRef.Cross(wRef.Cross(Dx) + Dv*2)
744  + wp.Cross(f) + w.Cross(w.Cross(f))));
745  memcpy(&iobuf_xpp[3*i], xppTilde.pGetVec(), 3*sizeof(doublereal));
746 
747  if (uRot != MBC_ROT_NONE) {
748  Vec3 wpTilde(RRef.MulTV(wp) - wpRef - wRef.Cross(w));
749  memcpy(&iobuf_omegap[3*i], wpTilde.pGetVec(), 3*sizeof(doublereal));
750  }
751  }
752  }
753 
754  } else {
755  for (unsigned i = 0; i < m_Points.size(); i++) {
756  /*
757  p = x + f
758  R = R
759  v = xp + w cross f
760  w = w
761  a = xpp + wp cross f + w cross w cross f
762  wp = wp
763  */
764 
765  // Optimization of the above formulas
766  const PointData& point = m_Points[i];
767  const Mat3x3& R = point.pNode->GetRCurr();
768  Vec3 f = R*point.Offset;
769  Vec3 x = point.pNode->GetXCurr() + f;
770  const Vec3& w = point.pNode->GetWCurr();
771  Vec3 wCrossf = w.Cross(f);
772  Vec3 v = point.pNode->GetVCurr() + wCrossf;
773 
774  if (bLabels) {
775  uint32_t l = point.pNode->GetLabel();
776  iobuf_labels[i] = l;
777  }
778  memcpy(&iobuf_x[3*i], x.pGetVec(), 3*sizeof(doublereal));
779  switch (uRot) {
780  case MBC_ROT_NONE:
781  break;
782 
783  case MBC_ROT_MAT:
784  memcpy(&iobuf_R[9*i], R.pGetMat(), 9*sizeof(doublereal));
785  break;
786 
787  case MBC_ROT_THETA: {
788  Vec3 Theta(RotManip::VecRot(R));
789  memcpy(&iobuf_theta[3*i], Theta.pGetVec(), 3*sizeof(doublereal));
790  } break;
791 
792  case MBC_ROT_EULER_123: {
793  Vec3 E(MatR2EulerAngles123(R)*dRaDegr);
794  memcpy(&iobuf_euler_123[3*i], E.pGetVec(), 3*sizeof(doublereal));
795  } break;
796  }
797  memcpy(&iobuf_xp[3*i], v.pGetVec(), 3*sizeof(doublereal));
798  if (uRot != MBC_ROT_NONE) {
799  memcpy(&iobuf_omega[3*i], w.pGetVec(), 3*sizeof(doublereal));
800  }
801 
802  if (bOutputAccelerations) {
803  const Vec3& wp = point.pNode->GetWPCurr();
804  Vec3 xpp = point.pNode->GetXPPCurr() + wp.Cross(f) + w.Cross(wCrossf);
805 
806  memcpy(&iobuf_xpp[3*i], xpp.pGetVec(), 3*sizeof(doublereal));
807  if (uRot != MBC_ROT_NONE) {
808  memcpy(&iobuf_omegap[3*i], wp.pGetVec(), 3*sizeof(doublereal));
809  }
810  }
811  }
812  }
813 
814  send(outfd, &iobuf[0], iobuf.size(), 0);
815 #else // ! USE_SOCKET
816  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
817 #endif // ! USE_SOCKET
818 }
819 
820 void
821 StructExtForce::Recv(ExtFileHandlerBase *pEFH)
822 {
823  std::istream *infp = pEFH->GetInStream();
824  if (infp) {
825  RecvFromStream(*infp);
826 
827  } else {
828  RecvFromFileDes(pEFH->GetInFileDes());
829  }
830 }
831 
832 void
833 StructExtForce::RecvFromStream(std::istream& inf)
834 {
835  if (pRefNode) {
836  doublereal *f = F0.pGetVec(), *m = M0.pGetVec();
837 
838  if (bLabels) {
839  // TODO: implement "unsorted"
840  unsigned l;
841  inf >> l;
842  }
843 
844  inf >> f[0] >> f[1] >> f[2];
845  if (uRot != MBC_ROT_NONE) {
846  inf >> m[0] >> m[1] >> m[2];
847  }
848  }
849 
850  if (!bSorted) {
851  ASSERT(bLabels);
852 
853  done.resize(m_Points.size());
854  fill(done.begin(), done.end(), false);
855 
856  unsigned cnt;
857  for (cnt = 0; inf; cnt++) {
858  /* assume unsigned int label */
859  unsigned l, i;
860  doublereal f[3], m[3];
861 
862  inf >> l
863  >> f[0] >> f[1] >> f[2];
864  if (uRot != MBC_ROT_NONE) {
865  inf >> m[0] >> m[1] >> m[2];
866  }
867 
868  if (!inf) {
869  break;
870  }
871 
872  for (i = 0; i < m_Points.size(); i++) {
873  if (m_Points[i].uLabel == l) {
874  break;
875  }
876  }
877 
878  if (i == m_Points.size()) {
879  silent_cerr("StructExtForce"
880  "(" << GetLabel() << "): "
881  "unknown label " << l
882  << " as " << cnt << "-th node"
883  << std::endl);
884  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
885  }
886 
887  if (done[i]) {
888  silent_cerr("StructExtForce"
889  "(" << GetLabel() << "): "
890  "label " << l << " already done"
891  << std::endl);
892  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
893  }
894 
895  done[i] = true;
896 
897  m_Points[i].F = Vec3(f);
898  if (uRot != MBC_ROT_NONE) {
899  m_Points[i].M = Vec3(m);
900  }
901  }
902 
903  if (cnt != m_Points.size()) {
904  silent_cerr("StructExtForce(" << GetLabel() << "): "
905  "invalid number of nodes " << cnt
906  << std::endl);
907 
908  for (unsigned i = 0; i < m_Points.size(); i++) {
909  if (!done[i]) {
910  silent_cerr("StructExtForce"
911  "(" << GetLabel() << "): "
912  "label " << m_Points[i].uLabel << " node " << m_Points[i].pNode->GetLabel()
913  << " not done" << std::endl);
914  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
915  }
916  }
917 
918  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
919  }
920 
921  } else {
922  for (unsigned i = 0; i < m_Points.size(); i++) {
923  PointData& point = m_Points[i];
924 
925 
926  /* assume unsigned int label */
927  doublereal f[3], m[3];
928 
929  if (bLabels) {
930  unsigned l;
931  inf >> l;
932 
933  if (point.pNode->GetLabel() != l) {
934  silent_cerr("StructExtForce"
935  "(" << GetLabel() << "): "
936  "invalid " << i << "-th label " << l
937  << std::endl);
938  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
939  }
940  }
941 
942  inf
943  >> f[0] >> f[1] >> f[2];
944  if (uRot != MBC_ROT_NONE) {
945  inf >> m[0] >> m[1] >> m[2];
946  }
947 
948  if (!inf) {
949  break;
950  }
951 
952  point.F = Vec3(f);
953  if (uRot != MBC_ROT_NONE) {
954  point.M = Vec3(m);
955  }
956  }
957  }
958 }
959 
960 void
961 StructExtForce::RecvFromFileDes(int infd)
962 {
963 #ifdef USE_SOCKET
964  if (pRefNode) {
965  size_t ulen = 0;
966  char buf[2*sizeof(uint32_t) + 6*sizeof(doublereal)];
967  doublereal *f;
968  ssize_t len;
969 
970  if (bLabels) {
971  // to align with double
972  ulen = 2*sizeof(uint32_t);
973  }
974 
975  if (uRot != MBC_ROT_NONE) {
976  ulen += 6*sizeof(doublereal);
977 
978  } else {
979  ulen += 3*sizeof(doublereal);
980  }
981 
982  len = recv(infd, (void *)buf, ulen, 0);
983  if (len == -1) {
984  int save_errno = errno;
985  char *err_msg = strerror(save_errno);
986  silent_cerr("StructExtForce(" << GetLabel() << "): "
987  "recv() failed (" << save_errno << ": "
988  << err_msg << ")" << std::endl);
989  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
990 
991  } else if (unsigned(len) != ulen) {
992  silent_cerr("StructExtForce(" << GetLabel() << "): "
993  "recv() failed (got " << len << " of "
994  << ulen << " bytes)" << std::endl);
995  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
996  }
997 
998  if (bLabels) {
999  uint32_t *uint32_ptr = (uint32_t *)buf;
1000  unsigned l = uint32_ptr[0];
1001  if (l != pRefNode->GetLabel()) {
1002  silent_cerr("StructExtForce(" << GetLabel() << "): "
1003  "invalid reference node label "
1004  "(wanted " << pRefNode->GetLabel() << ", got " << l << ")"
1005  << std::endl);
1006  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1007  }
1008  f = (doublereal *)&uint32_ptr[2];
1009 
1010  } else {
1011  f = (doublereal *)buf;
1012  }
1013 
1014  F0 = Vec3(&f[0]);
1015  if (uRot != MBC_ROT_NONE) {
1016  M0 = Vec3(&f[3]);
1017  }
1018  }
1019 
1020  ssize_t len = recv(infd, (void *)&iobuf[0], dynamics_size, 0);
1021  if (len == -1) {
1022  int save_errno = errno;
1023  char *err_msg = strerror(save_errno);
1024  silent_cerr("StructExtForce(" << GetLabel() << "): "
1025  "recv() failed (" << save_errno << ": "
1026  << err_msg << ")" << std::endl);
1027  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1028 
1029  } else if (unsigned(len) != dynamics_size) {
1030  silent_cerr("StructExtForce(" << GetLabel() << "): "
1031  "recv() failed " "(got " << len << " of "
1032  << dynamics_size << " bytes)" << std::endl);
1033  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1034  }
1035 
1036  if (!bSorted) {
1037  ASSERT(bLabels);
1038 
1039  done.resize(m_Points.size());
1040  fill(done.begin(), done.end(), false);
1041 
1042  unsigned cnt;
1043  for (cnt = 0; cnt < m_Points.size(); cnt++) {
1044  PointData& point = m_Points[cnt];
1045 
1046  unsigned l = iobuf_labels[cnt];
1047  std::vector<PointData>::const_iterator p;
1048  for (p = m_Points.begin(); p != m_Points.end(); ++p) {
1049  if (p->uLabel == l) {
1050  break;
1051  }
1052  }
1053 
1054  if (p == m_Points.end()) {
1055  silent_cerr("StructExtForce"
1056  "(" << GetLabel() << "): "
1057  "unknown label " << l
1058  << " as " << cnt << "-th node"
1059  << std::endl);
1060  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1061  }
1062 
1063  unsigned i = p - m_Points.begin();
1064 
1065  if (done[i]) {
1066  silent_cerr("StructExtForce"
1067  "(" << GetLabel() << "): "
1068  "label " << l << " already done"
1069  << std::endl);
1070  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1071  }
1072 
1073  done[i] = true;
1074 
1075  point.F = &iobuf_f[3*i];
1076  if (uRot != MBC_ROT_NONE) {
1077  point.M = &iobuf_m[3*i];
1078  }
1079  }
1080 
1081  if (cnt != m_Points.size()) {
1082  silent_cerr("StructExtForce(" << GetLabel() << "): "
1083  "invalid node number " << cnt
1084  << std::endl);
1085 
1086  for (unsigned i = 0; i < m_Points.size(); i++) {
1087  if (!done[i]) {
1088  silent_cerr("StructExtForce"
1089  "(" << GetLabel() << "): "
1090  "label " << m_Points[i].uLabel << "node " << m_Points[i].pNode->GetLabel()
1091  << " not done" << std::endl);
1092  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1093  }
1094  }
1095 
1096  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1097  }
1098 
1099  } else {
1100  for (unsigned i = 0; i < m_Points.size(); i++) {
1101  PointData& point = m_Points[i];
1102 
1103  if (bLabels) {
1104  unsigned l = iobuf_labels[i];
1105  if (point.pNode->GetLabel() != l) {
1106  silent_cerr("StructExtForce"
1107  "(" << GetLabel() << "): "
1108  "invalid " << i << "-th label " << l
1109  << std::endl);
1110  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1111  }
1112  }
1113 
1114  point.F = Vec3(&iobuf_f[3*i]);
1115  if (uRot != MBC_ROT_NONE) {
1116  point.M = Vec3(&iobuf_m[3*i]);
1117  }
1118  }
1119  }
1120 #else // ! USE_SOCKET
1121  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1122 #endif // ! USE_SOCKET
1123 }
1124 
1125 SubVectorHandler&
1126 StructExtForce::AssRes(SubVectorHandler& WorkVec,
1127  doublereal dCoef,
1128  const VectorHandler& XCurr,
1129  const VectorHandler& XPrimeCurr)
1130 {
1131  ExtForce::Recv();
1132 
1133  if (iCoupling == COUPLING_NONE) {
1134  WorkVec.Resize(0);
1135  return WorkVec;
1136  }
1137 
1138  int iOffset;
1139  if (uRot != MBC_ROT_NONE) {
1140  iOffset = 6;
1141 
1142  } else {
1143  iOffset = 3;
1144  }
1145 
1146  if (pRefNode) {
1147  integer iSize = m_Points.size();
1148  if (bUseReferenceNodeForces) {
1149  iSize++;
1150  }
1151 
1152  WorkVec.ResizeReset(iOffset*iSize);
1153 
1154  const Vec3& xRef = pRefNode->GetXCurr();
1155  const Mat3x3& RRef = pRefNode->GetRCurr();
1156 
1157  // manipulate
1158  if (bUseReferenceNodeForces) {
1159  if (bRotateReferenceNodeForces) {
1160  F1 = RRef*F0;
1161  if (uRot != MBC_ROT_NONE) {
1162  M1 = RRef*M0;
1163  }
1164 
1165  } else {
1166  F1 = F0;
1167  if (uRot != MBC_ROT_NONE) {
1168  M1 = M0;
1169  }
1170  }
1171  }
1172 
1173  F2 = Zero3;
1174  M2 = Zero3;
1175  for (unsigned i = 0; i < m_Points.size(); i++) {
1176  const PointData& point = m_Points[i];
1177 
1178  integer iFirstIndex = point.pNode->iGetFirstMomentumIndex();
1179  for (int r = 1; r <= iOffset; r++) {
1180  WorkVec.PutRowIndex(i*iOffset + r, iFirstIndex + r);
1181  }
1182 
1183  Vec3 f(RRef*point.F);
1184  WorkVec.Add(i*iOffset + 1, f);
1185 
1186  Vec3 m;
1187  if (uRot != MBC_ROT_NONE) {
1188  m = RRef*point.M + (point.pNode->GetRCurr()*point.Offset).Cross(f);
1189  WorkVec.Add(i*iOffset + 4, m);
1190  }
1191 
1192  if (bUseReferenceNodeForces || bToBeOutput()) {
1193  F2 += f;
1194  if (uRot != MBC_ROT_NONE) {
1195  M2 += m + (point.pNode->GetXCurr() - xRef).Cross(f);
1196  }
1197  }
1198  }
1199 
1200  if (bUseReferenceNodeForces) {
1201  unsigned i = m_Points.size();
1202  integer iFirstIndex = pRefNode->iGetFirstMomentumIndex();
1203  for (int r = 1; r <= iOffset; r++) {
1204  WorkVec.PutRowIndex(i*iOffset + r, iFirstIndex + r);
1205  }
1206 
1207  F1 -= F2;
1208  M1 -= M2;
1209 
1210  WorkVec.Add(i*iOffset + 1, F1);
1211  if (uRot != MBC_ROT_NONE) {
1212  WorkVec.Add(i*iOffset + 4, M1);
1213  }
1214  }
1215 
1216  } else {
1217  WorkVec.ResizeReset(iOffset*m_Points.size());
1218 
1219  for (unsigned i = 0; i < m_Points.size(); i++) {
1220  const PointData& point = m_Points[i];
1221 
1222  integer iFirstIndex = point.pNode->iGetFirstMomentumIndex();
1223  for (int r = 1; r <= iOffset; r++) {
1224  WorkVec.PutRowIndex(i*iOffset + r, iFirstIndex + r);
1225  }
1226 
1227  WorkVec.Add(i*iOffset + 1, point.F);
1228  if (uRot != MBC_ROT_NONE) {
1229  WorkVec.Add(i*iOffset + 4, point.M + (point.pNode->GetRCurr()*point.Offset).Cross(point.F));
1230  }
1231  }
1232  }
1233 
1234  return WorkVec;
1235 }
1236 
1237 void
1238 StructExtForce::Output(OutputHandler& OH) const
1239 {
1240  if (bToBeOutput()) {
1241  if (OH.UseText(OutputHandler::FORCES)) {
1242  std::ostream& out = OH.Forces();
1243 
1244  if (pRefNode && (uOutputFlags & ExtFileHandlerBase::OUTPUT_REF_DYN)) {
1245  out << GetLabel() << "#" << pRefNode->GetLabel()
1246  << " " << F0
1247  << " " << M0
1248  << " " << F1
1249  << " " << M1
1250  << " " << F2
1251  << " " << M2
1252  << std::endl;
1253  }
1254 
1255  if (uOutputFlags & ExtFileHandlerBase::OUTPUT_DYN) {
1256  for (std::vector<PointData>::const_iterator point = m_Points.begin(); point != m_Points.end(); ++point) {
1257  out << GetLabel() << "@" << point->uLabel
1258  << " " << point->F
1259  << " " << point->M
1260  << std::endl;
1261  }
1262  }
1263 
1264  if (uOutputFlags & ExtFileHandlerBase::OUTPUT_KIN) {
1265  if (pRefNode) {
1266  const Vec3& xRef = pRefNode->GetXCurr();
1267  const Mat3x3& RRef = pRefNode->GetRCurr();
1268  const Vec3& xpRef = pRefNode->GetVCurr();
1269  const Vec3& wRef = pRefNode->GetWCurr();
1270  //const Vec3& xppRef = pRefNode->GetXPPCurr();
1271  //const Vec3& wpRef = pRefNode->GetWPCurr();
1272 
1273  for (std::vector<PointData>::const_iterator point = m_Points.begin(); point != m_Points.end(); ++point) {
1274  Vec3 f(point->pNode->GetRCurr()*point->Offset);
1275  Vec3 x(point->pNode->GetXCurr() + f);
1276  Vec3 Dx(x - xRef);
1277  Mat3x3 DR(RRef.MulTM(point->pNode->GetRCurr()));
1278  Vec3 v(point->pNode->GetVCurr() + point->pNode->GetWCurr().Cross(f));
1279  Vec3 Dv(v - xpRef - wRef.Cross(Dx));
1280  const Vec3& w(point->pNode->GetWCurr());
1281 
1282  out << GetLabel() << "." << point->uLabel
1283  << " " << RRef.MulTV(Dx);
1284 
1285  switch (uRot) {
1286  case MBC_ROT_MAT:
1287  out << " " << DR;
1288  break;
1289 
1290  case MBC_ROT_THETA:
1291  out << " " << RotManip::VecRot(DR);
1292  break;
1293 
1294  case MBC_ROT_NONE:
1295  case MBC_ROT_EULER_123:
1296  out << " " << MatR2EulerAngles123(DR)*dRaDegr;
1297  break;
1298  }
1299 
1300  out << " " << RRef.MulTV(Dv)
1301  << " " << RRef.MulTV(w - wRef)
1302  << std::endl;
1303  }
1304 
1305  } else {
1306  for (std::vector<PointData>::const_iterator point = m_Points.begin(); point != m_Points.end(); ++point) {
1307  const Mat3x3& R = point->pNode->GetRCurr();
1308  Vec3 f = R*point->Offset;
1309  Vec3 x = point->pNode->GetXCurr() + f;
1310  const Vec3& w = point->pNode->GetWCurr();
1311  Vec3 wCrossf = w.Cross(f);
1312  Vec3 v = point->pNode->GetVCurr() + wCrossf;
1313 
1314  out << GetLabel() << "." << point->uLabel
1315  << " " << x;
1316 
1317  switch (uRot) {
1318  case MBC_ROT_MAT:
1319  out << " " << R;
1320  break;
1321 
1322  case MBC_ROT_THETA:
1323  out << " " << RotManip::VecRot(R);
1324  break;
1325 
1326  case MBC_ROT_NONE:
1327  case MBC_ROT_EULER_123:
1328  out << " " << MatR2EulerAngles123(R)*dRaDegr;
1329  break;
1330  }
1331  out << " " << v
1332  << " " << w
1333  << std::endl;
1334  }
1335  }
1336  }
1337  }
1338 
1339  /* TODO: NetCDF */
1340  }
1341 }
1342 
1343 void
1344 StructExtForce::GetConnectedNodes(std::vector<const Node *>& connectedNodes) const {
1345  connectedNodes.resize(m_Points.size());
1346  for (unsigned int i = 0; i < m_Points.size(); i++) {
1347  connectedNodes[i] = m_Points[i].pNode;
1348  }
1349 }
1350 
1351 Elem*
1352 ReadStructExtForce(DataManager* pDM,
1353  MBDynParser& HP,
1354  unsigned int uLabel)
1355 {
1356  ExtFileHandlerBase *pEFH = 0;
1357  int iCoupling;
1358 
1359  bool bSendAfterPredict;
1360  ReadExtForce(pDM, HP, uLabel, pEFH, bSendAfterPredict, iCoupling);
1361 
1362  const StructNode *pRefNode(0);
1363  if (HP.IsKeyWord("reference" "node")) {
1364  pRefNode = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
1365  }
1366 
1367  bool bSorted(true);
1368  bool bLabels(false);
1369  unsigned uRot = MBC_ROT_MAT;
1370  bool bOutputAccelerations(false);
1371  bool bUseReferenceNodeForces(true);
1372  bool bRotateReferenceNodeForces(true);
1373 
1374  bool bGotSorted(false);
1375  bool bGotLabels(false);
1376  bool bGotRot(false);
1377  bool bGotAccels(false);
1378  bool bGotUseRefForces(false);
1379 
1380  while (HP.IsArg()) {
1381  if (HP.IsKeyWord("unsorted")) {
1382  silent_cerr("StructExtForce(" << uLabel << "): "
1383  "use of \"unsorted\" deprecated in favor of \"sorted, { yes | no }\" at line "
1384  << HP.GetLineData() << std::endl);
1385 
1386  if (bGotSorted) {
1387  silent_cerr("StructExtForce(" << uLabel << "): "
1388  "\"unsorted\" already specified at line "
1389  << HP.GetLineData() << std::endl);
1390  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1391  }
1392 
1393  bSorted = false;
1394  bGotSorted = true;
1395 
1396  } else if (HP.IsKeyWord("sorted")) {
1397  if (bGotSorted) {
1398  silent_cerr("StructExtForce(" << uLabel << "): "
1399  "\"sorted\" already specified at line "
1400  << HP.GetLineData() << std::endl);
1401  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1402  }
1403 
1404  if (!HP.GetYesNo(bSorted)) {
1405  silent_cerr("StructExtForce(" << uLabel << "): "
1406  "\"sorted\" must be either \"yes\" or \"no\" at line "
1407  << HP.GetLineData() << std::endl);
1408  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1409  }
1410  bGotSorted = true;
1411 
1412  } else if (HP.IsKeyWord("no" "labels")) {
1413  silent_cerr("StructExtForce(" << uLabel << "): "
1414  "use of \"no labels\" deprecated in favor of \"labels, { yes | no }\" at line "
1415  << HP.GetLineData() << std::endl);
1416 
1417  if (bGotLabels) {
1418  silent_cerr("StructExtForce(" << uLabel << "): "
1419  "\"no labels\" already specified at line "
1420  << HP.GetLineData() << std::endl);
1421  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1422  }
1423 
1424  bLabels = false;
1425  bGotLabels = true;
1426 
1427  } else if (HP.IsKeyWord("labels")) {
1428  if (bGotLabels) {
1429  silent_cerr("StructExtForce(" << uLabel << "): "
1430  "\"labels\" already specified at line "
1431  << HP.GetLineData() << std::endl);
1432  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1433  }
1434 
1435  if (!HP.GetYesNo(bLabels)) {
1436  silent_cerr("StructExtForce(" << uLabel << "): "
1437  "\"labels\" must be either \"yes\" or \"no\" at line "
1438  << HP.GetLineData() << std::endl);
1439  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1440  }
1441  bGotLabels = true;
1442 
1443  } else if (HP.IsKeyWord("orientation")) {
1444  if (bGotRot) {
1445  silent_cerr("StructExtForce(" << uLabel << "): "
1446  "\"orientation\" already specified at line "
1447  << HP.GetLineData() << std::endl);
1448  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1449  }
1450 
1451  if (HP.IsKeyWord("none")) {
1452  uRot = MBC_ROT_NONE;
1453 
1454  } else if (HP.IsKeyWord("orientation" "vector")) {
1455  uRot = MBC_ROT_THETA;
1456 
1457  } else if (HP.IsKeyWord("orientation" "matrix")) {
1458  uRot = MBC_ROT_MAT;
1459 
1460  } else if (HP.IsKeyWord("euler" "123")) {
1461  uRot = MBC_ROT_EULER_123;
1462 
1463  } else {
1464  silent_cerr("StructExtForce(" << uLabel << "): "
1465  "unknown \"orientation\" format at line "
1466  << HP.GetLineData() << std::endl);
1467  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1468  }
1469 
1470  bGotRot = true;
1471 
1472  } else if (HP.IsKeyWord("accelerations")) {
1473  if (bGotAccels) {
1474  silent_cerr("StructExtForce(" << uLabel << "): "
1475  "\"accelerations\" already specified at line "
1476  << HP.GetLineData() << std::endl);
1477  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1478  }
1479 
1480  if (!HP.GetYesNo(bOutputAccelerations)) {
1481  silent_cerr("StructExtForce(" << uLabel << "): "
1482  "\"accelerations\" must be either \"yes\" or \"no\" at line "
1483  << HP.GetLineData() << std::endl);
1484  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1485  }
1486  bGotAccels = true;
1487 
1488  } else if (HP.IsKeyWord("use" "reference" "node" "forces")) {
1489  if (pRefNode == 0) {
1490  silent_cerr("StructExtForce(" << uLabel << "): "
1491  "\"use reference node forces\" only meaningful when reference node is used at line "
1492  << HP.GetLineData() << std::endl);
1493  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1494  }
1495 
1496  if (bGotUseRefForces) {
1497  silent_cerr("StructExtForce(" << uLabel << "): "
1498  "\"use reference node forces\" already specified at line "
1499  << HP.GetLineData() << std::endl);
1500  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1501  }
1502 
1503  if (!HP.GetYesNo(bUseReferenceNodeForces)) {
1504  silent_cerr("StructExtForce(" << uLabel << "): "
1505  "\"use reference node forces\" must be either \"yes\" or \"no\" at line "
1506  << HP.GetLineData() << std::endl);
1507  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1508  }
1509  bGotUseRefForces = true;
1510 
1511  if (bUseReferenceNodeForces && HP.IsKeyWord("rotate" "reference" "node" "forces")) {
1512  if (!HP.GetYesNo(bRotateReferenceNodeForces)) {
1513  silent_cerr("StructExtForce(" << uLabel << "): "
1514  "\"rotate reference node forces\" must be either \"yes\" or \"no\" at line "
1515  << HP.GetLineData() << std::endl);
1516  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1517  }
1518  }
1519 
1520  } else {
1521  break;
1522  }
1523  }
1524 
1525  if (!bLabels && !bSorted) {
1526  silent_cerr("StructExtForce(" << uLabel << "): "
1527  "\"no labels\" and \"unsorted\" incompatible" << std::endl);
1528  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1529  }
1530 
1531  int n = HP.GetInt();
1532  if (n <= 0) {
1533  silent_cerr("StructExtForce(" << uLabel << "): illegal node number " << n <<
1534  " at line " << HP.GetLineData() << std::endl);
1535  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1536  }
1537 
1538  std::vector<unsigned> Labels;
1539  std::vector<unsigned>::iterator curr_label;
1540  if (bLabels) {
1541  Labels.resize(n);
1542  curr_label = Labels.begin();
1543  }
1544  std::vector<const StructNode *> Nodes(n);
1545  std::vector<Vec3> Offsets(n);
1546 
1547  for (int i = 0; i < n; i++ ) {
1548  Nodes[i] = pDM->ReadNode<const StructNode, Node::STRUCTURAL>(HP);
1549 
1550  ReferenceFrame RF(Nodes[i]);
1551 
1552  if (bLabels) {
1553  unsigned ul;
1554  if (HP.IsKeyWord("label")) {
1555  int l = HP.GetInt();
1556  if (l < 0) {
1557  silent_cerr("StructExtForce(" << uLabel << "): "
1558  "invalid label for point #" << i << " at line "
1559  << HP.GetLineData() << std::endl);
1560  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1561  }
1562  ul = unsigned(l);
1563 
1564  } else {
1565  ul = Nodes[i]->GetLabel();
1566  }
1567 
1568  if (std::find(Labels.begin(), curr_label, ul) < curr_label) {
1569  silent_cerr("StructExtForce(" << uLabel << "): "
1570  "duplicate label for point #" << i << " at line "
1571  << HP.GetLineData() << std::endl);
1572  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1573  }
1574 
1575  *curr_label = ul;
1576  ++curr_label;
1577 
1578  }
1579 
1580  if (HP.IsKeyWord("offset")) {
1581  Offsets[i] = HP.GetPosRel(RF);
1582 
1583  } else {
1584  Offsets[i] = Vec3(Zero3);
1585  }
1586 
1587  for (int j = 0; j < i; j++) {
1588  if (Nodes[j] == Nodes[i]) {
1589  if (Offsets[j].IsExactlySame(Offsets[i])) {
1590  silent_cerr("StructExtForce(" << uLabel << "): "
1591  "warning, point #" << i << " is identical to point #" << j << " (same node, same offset)" << std::endl);
1592  } else {
1593  silent_cerr("StructExtForce(" << uLabel << "): "
1594  "warning, point #" << i << " is based on same node of point #" << j << " (offsets differ)" << std::endl);
1595  }
1596  }
1597  }
1598  }
1599 
1600  std::ofstream out;
1601  if (HP.IsKeyWord("echo")) {
1602  const char *s = HP.GetFileName();
1603  if (s == NULL) {
1604  silent_cerr("StructMappingExtForce(" << uLabel << "): "
1605  "unable to parse echo file name "
1606  "at line " << HP.GetLineData()
1607  << std::endl);
1608  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1609  }
1610 
1611  out.open(s);
1612  if (!out) {
1613  int save_errno = errno;
1614  silent_cerr("StructMappingExtForce(" << uLabel << "): "
1615  "unable to open file \"" << s << "\" (" << save_errno << ": " << strerror(save_errno) << ")" << std::endl);
1616  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1617  }
1618 #if 0
1619 
1620  std::ofstream out(s);
1621 
1622  out.setf(std::ios::scientific);
1623 
1624  for (unsigned i = 0; i < Nodes.size(); i++) {
1625  if (bLabels) {
1626  out << Labels[i];
1627  } else {
1628  out << Nodes[i]->GetLabel();
1629  }
1630 
1631  out << " " << (Nodes[i]->GetXCurr() + Offsets[i]) << " ";
1632  switch (uRot) {
1633  case MBC_ROT_MAT:
1634  out << Nodes[i]->GetRCurr();
1635  break;
1636 
1637  case MBC_ROT_THETA:
1638  out << RotManip::VecRot(Nodes[i]->GetRCurr());
1639  break;
1640 
1641  case MBC_ROT_NONE:
1642  case MBC_ROT_EULER_123:
1643  out << MatR2EulerAngles123(Nodes[i]->GetRCurr())*dRaDegr;
1644  break;
1645  }
1646  out << " " << Nodes[i]->GetVCurr() + Nodes[i]->GetWCurr().Cross(Offsets[i])
1647  << " " << Nodes[i]->GetWCurr()
1648  << std::endl;
1649  }
1650 #endif
1651  }
1652 
1653  unsigned uOutputFlags = ExtFileHandlerBase::OUTPUT_REF_DYN;
1654  flag fOut = pDM->fReadOutput(HP, Elem::FORCE);
1655  if (HP.IsArg()) {
1656  if (HP.IsKeyWord("kinematics")) {
1657  uOutputFlags |= ExtFileHandlerBase::OUTPUT_KIN;
1658  fOut = 1;
1659  } else {
1660  silent_cerr("StructMappingExtForce(" << uLabel << "): "
1661  "unexpected arg at line " << HP.GetLineData() << std::endl);
1662  throw ErrGeneric(MBDYN_EXCEPT_ARGS);
1663  }
1664  }
1665 
1666  StructExtForce *pEl = 0;
1667 
1668  if (dynamic_cast<ExtFileHandlerEDGE *>(pEFH)) {
1669  SAFENEWWITHCONSTRUCTOR(pEl, StructExtEDGEForce,
1670  StructExtEDGEForce(uLabel, pDM, pRefNode,
1671  bUseReferenceNodeForces, bRotateReferenceNodeForces,
1672  Labels, Nodes, Offsets,
1673  bSorted, bLabels, bOutputAccelerations, uRot,
1674  pEFH, bSendAfterPredict, iCoupling, uOutputFlags, fOut));
1675 
1676  } else {
1677  SAFENEWWITHCONSTRUCTOR(pEl, StructExtForce,
1678  StructExtForce(uLabel, pDM, pRefNode,
1679  bUseReferenceNodeForces, bRotateReferenceNodeForces,
1680  Labels, Nodes, Offsets,
1681  bSorted, bLabels, bOutputAccelerations, uRot,
1682  pEFH, bSendAfterPredict, iCoupling, uOutputFlags, fOut));
1683  }
1684 
1685  if (out.is_open()) {
1686  pEl->SendToStream(out, ExtFileHandlerBase::SEND_FIRST_TIME);
1687  }
1688 
1689  return pEl;
1690 }
1691 
DataManager::fReadOutput
flag fReadOutput(MBDynParser &HP, const T &t) const
Definition: dataman.h:1064
StructExtForce::iobuf_R
doublereal * iobuf_R
Definition: strext.h:81
StructExtForce::SendToFileDes
virtual void SendToFileDes(int outfd, ExtFileHandlerBase::SendWhen when)
Definition: strext.cc:644
ExtFileHandlerBase::GetInStream
virtual std::istream * GetInStream(void)
Definition: extforce.cc:78
Zero3
const Vec3 Zero3(0., 0., 0.)
Vec3::Cross
Vec3 Cross(const Vec3 &v) const
Definition: matvec3.h:218
StructExtForce::iobuf_m
doublereal * iobuf_m
Definition: strext.h:89
flag
long int flag
Definition: mbdyn.h:43
ToBeOutput::bToBeOutput
virtual bool bToBeOutput(void) const
Definition: output.cc:890
StructExtForce::iobuf_xp
doublereal * iobuf_xp
Definition: strext.h:84
MBDYN_EXCEPT_ARGS
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
Vec3
Definition: matvec3.h:98
ExtFileHandlerBase::GetInFileDes
virtual int GetInFileDes(void)
Definition: extforce.cc:99
StructExtForce::iobuf
std::vector< char > iobuf
Definition: strext.h:78
ExtFileHandlerBase::GetOutFileDes
virtual int GetOutFileDes(void)
Definition: extforce.cc:85
VectorHandler::ResizeReset
virtual void ResizeReset(integer)
Definition: vh.cc:55
ExtForce::Recv
void Recv(void)
Definition: extforce.cc:798
HighParser::GetInt
virtual integer GetInt(integer iDefval=0)
Definition: parser.cc:1050
StructExtForce::iobuf_euler_123
doublereal * iobuf_euler_123
Definition: strext.h:83
StructNode::GetRCurr
virtual const Mat3x3 & GetRCurr(void) const
Definition: strnode.h:1012
StructExtForce::PointData::pNode
const StructNode * pNode
Definition: strext.h:60
StructExtForce::GetConnectedNodes
virtual void GetConnectedNodes(std::vector< const Node * > &connectedNodes) const
Definition: strext.cc:1344
IncludeParser::GetFileName
virtual const char * GetFileName(enum Delims Del=DEFAULTDELIM)
Definition: parsinc.cc:673
StructExtForce::done
std::vector< bool > done
Definition: strext.h:71
StructExtForce::StructExtForce
StructExtForce(unsigned int uL, DataManager *pDM, const StructNode *pRefNode, bool bUseReferenceNodeForces, bool bRotateReferenceNodeForces, std::vector< unsigned > &Labels, std::vector< const StructNode * > &Nodes, std::vector< Vec3 > &Offsets, bool bSorted, bool bLabels, bool bOutputAccelerations, unsigned bRot, ExtFileHandlerBase *pEFH, bool bSendAfterPredict, int iCoupling, unsigned uOutputFlags, flag fOut)
Definition: strext.cc:47
StructExtForce::bLabels
bool bLabels
Definition: strext.h:69
StructExtForce::WorkSpaceDim
void WorkSpaceDim(integer *piNumRows, integer *piNumCols) const
Definition: strext.cc:253
NO_OP
#define NO_OP
Definition: myassert.h:74
StructExtForce::Send
void Send(ExtFileHandlerBase *pEFH, ExtFileHandlerBase::SendWhen when)
Definition: strext.cc:430
MBC_ACCELS
Definition: mbc.h:71
StructExtForce::Prepare
bool Prepare(ExtFileHandlerBase *pEFH)
Definition: strext.cc:266
RotManip::VecRot
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
StructExtForce::Output
virtual void Output(OutputHandler &OH) const
Definition: strext.cc:1238
Mat3x3::MulTV
Vec3 MulTV(const Vec3 &v) const
Definition: matvec3.cc:482
ExtFileHandlerBase::NegotiateRequest
virtual Negotiate NegotiateRequest(void) const
Definition: extforce.cc:63
SubVectorHandler::PutRowIndex
virtual void PutRowIndex(integer iSubRow, integer iRow)=0
StructExtForce::~StructExtForce
virtual ~StructExtForce(void)
Definition: strext.cc:247
ReadStructExtForce
Elem * ReadStructExtForce(DataManager *pDM, MBDynParser &HP, unsigned int uLabel)
Definition: strext.cc:1352
StructExtForce::bOutputAccelerations
bool bOutputAccelerations
Definition: strext.h:74
labels
static int labels
Definition: test_strext_socket_lib.c:88
MBC_NODAL
Definition: mbc.h:66
MBDynParser::GetPosRel
Vec3 GetPosRel(const ReferenceFrame &rf)
Definition: mbpar.cc:1331
StructExtForce::bSorted
bool bSorted
Definition: strext.h:70
HighParser::IsKeyWord
virtual bool IsKeyWord(const char *sKeyWord)
Definition: parser.cc:910
StructExtForce::m_Points
std::vector< PointData > m_Points
Definition: strext.h:67
StructExtForce::iobuf_theta
doublereal * iobuf_theta
Definition: strext.h:82
StructExtForce::iobuf_xpp
doublereal * iobuf_xpp
Definition: strext.h:86
ExtFileHandlerBase::GetRecvFlags
virtual int GetRecvFlags(void) const
Definition: extforce.cc:106
MatR2EulerAngles123
Vec3 MatR2EulerAngles123(const Mat3x3 &R)
Definition: matvec3.cc:893
StructExtForce::bUseReferenceNodeForces
bool bUseReferenceNodeForces
Definition: strext.h:50
StructDispNode::iGetFirstMomentumIndex
virtual integer iGetFirstMomentumIndex(void) const =0
StructNode::GetWCurr
virtual const Vec3 & GetWCurr(void) const
Definition: strnode.h:1030
dRaDegr
const doublereal dRaDegr
Definition: matvec3.cc:884
WithLabel::uLabel
unsigned int uLabel
Definition: withlab.h:44
StructNode::GetWPCurr
virtual const Vec3 & GetWPCurr(void) const
Definition: strnode.h:1042
ASSERT
#define ASSERT(expression)
Definition: colamd.c:977
ExtFileHandlerBase::GetOutStream
virtual std::ostream * GetOutStream(void)
Definition: extforce.cc:72
Mat3x3::MulTM
Mat3x3 MulTM(const Mat3x3 &m) const
Definition: matvec3.cc:500
grad::Cross
VectorExpression< VectorCrossExpr< VectorLhsExpr, VectorRhsExpr >, 3 > Cross(const VectorExpression< VectorLhsExpr, 3 > &u, const VectorExpression< VectorRhsExpr, 3 > &v)
Definition: matvec.h:3248
StructExtForce::SendToStream
virtual void SendToStream(std::ostream &outf, ExtFileHandlerBase::SendWhen when)
Definition: strext.cc:442
SAFENEWWITHCONSTRUCTOR
#define SAFENEWWITHCONSTRUCTOR(pnt, item, constructor)
Definition: mynewmem.h:698
StructDispNode::GetXCurr
virtual const Vec3 & GetXCurr(void) const
Definition: strnode.h:310
VectorHandler::Add
virtual void Add(integer iRow, const Vec3 &v)
Definition: vh.cc:63
nodes
static int nodes
Definition: test_strext_socket_lib.c:87
StructExtForce::RecvFromStream
virtual void RecvFromStream(std::istream &inf)
Definition: strext.cc:833
Mat3x3::pGetMat
const doublereal * pGetMat(void) const
Definition: matvec3.h:743
HighParser::GetYesNo
virtual bool GetYesNo(bool &bRet)
Definition: parser.cc:1022
HighParser::IsArg
virtual bool IsArg(void)
Definition: parser.cc:807
Elem
Definition: elem.h:75
StructExtForce::AssRes
SubVectorHandler & AssRes(SubVectorHandler &WorkVec, doublereal dCoef, const VectorHandler &XCurr, const VectorHandler &XPrimeCurr)
Definition: strext.cc:1126
StructExtForce::uRot
unsigned uRot
Definition: strext.h:73
StructExtForce::iobuf_omegap
doublereal * iobuf_omegap
Definition: strext.h:87
Vec3::pGetVec
const doublereal * pGetVec(void) const
Definition: matvec3.h:192
StructDispNode::GetVCurr
virtual const Vec3 & GetVCurr(void) const
Definition: strnode.h:322
a
static const doublereal a
Definition: hfluid_.h:289
ExtFileHandlerBase::GetSendFlags
virtual int GetSendFlags(void) const
Definition: extforce.cc:92
StructDispNode::GetXPPCurr
virtual const Vec3 & GetXPPCurr(void) const
Definition: strnode.h:334
StructExtForce::dynamics_size
unsigned dynamics_size
Definition: strext.h:77
buf
static doublereal buf[BUFSIZE]
Definition: discctrl.cc:333
doublereal
double doublereal
Definition: colamd.c:52
StructExtForce::iobuf_labels
uint32_t * iobuf_labels
Definition: strext.h:79
ExtForce::iCoupling
int iCoupling
Definition: extforce.h:212
integer
long int integer
Definition: colamd.c:51
StructExtForce::iobuf_omega
doublereal * iobuf_omega
Definition: strext.h:85
StructExtForce::bRotateReferenceNodeForces
bool bRotateReferenceNodeForces
Definition: strext.h:51
IncludeParser::GetLineData
virtual HighParser::ErrOut GetLineData(void) const
Definition: parsinc.cc:697
StructExtForce::uOutputFlags
unsigned uOutputFlags
Definition: strext.h:55
MBC_LABELS
Definition: mbc.h:73
WithLabel::GetLabel
unsigned int GetLabel(void) const
Definition: withlab.cc:62
DataManager::ReadNode
Node * ReadNode(MBDynParser &HP, Node::Type type) const
Definition: dataman3.cc:2309
StructExtForce::pRefNode
const StructNode * pRefNode
Definition: strext.h:49
ReadExtForce
void ReadExtForce(DataManager *pDM, MBDynParser &HP, unsigned int uLabel, ExtFileHandlerBase *&pEFH, bool &bSendAfterPredict, int &iCoupling)
Definition: extforce.cc:1141
StructExtForce::iobuf_x
doublereal * iobuf_x
Definition: strext.h:80
VectorHandler::Resize
virtual void Resize(integer iNewSize)=0
OutputHandler::Forces
std::ostream & Forces(void) const
Definition: output.h:450
R
Mat3x3 R
Definition: mbdyn_util_oct.cc:74
StructExtForce::iobuf_f
doublereal * iobuf_f
Definition: strext.h:88
OutputHandler::UseText
bool UseText(int out) const
Definition: output.cc:446
StructExtForce::RecvFromFileDes
virtual void RecvFromFileDes(int infd)
Definition: strext.cc:961
IsExactlySame
bool IsExactlySame(const doublereal &d1, const doublereal &d2)
Definition: matvec3.cc:1131