rigidBodyModel.C
Go to the documentation of this file.
1 /*---------------------------------------------------------------------------*\
2  ========= |
3  \\ / F ield | OpenFOAM: The Open Source CFD Toolbox
4  \\ / O peration | Website: https://openfoam.org
5  \\ / A nd | Copyright (C) 2016-2021 OpenFOAM Foundation
6  \\/ M anipulation |
7 -------------------------------------------------------------------------------
8 License
9  This file is part of OpenFOAM.
10 
11  OpenFOAM is free software: you can redistribute it and/or modify it
12  under the terms of the GNU General Public License as published by
13  the Free Software Foundation, either version 3 of the License, or
14  (at your option) any later version.
15 
16  OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
17  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
18  FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
19  for more details.
20 
21  You should have received a copy of the GNU General Public License
22  along with OpenFOAM. If not, see <http://www.gnu.org/licenses/>.
23 
24 \*---------------------------------------------------------------------------*/
25 
26 #include "rigidBodyModel.H"
27 #include "masslessBody.H"
28 #include "compositeBody.H"
29 #include "jointBody.H"
30 #include "nullJoint.H"
31 #include "rigidBodyRestraint.H"
32 
33 // * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
34 
35 namespace Foam
36 {
37 namespace RBD
38 {
39  defineTypeNameAndDebug(rigidBodyModel, 0);
40 }
41 }
42 
43 
44 // * * * * * * * * * * * * * Private Member Functions * * * * * * * * * * * //
45 
46 void Foam::RBD::rigidBodyModel::initialiseRootBody()
47 {
48  bodies_.append(new masslessBody("root"));
49  lambda_.append(0);
50  bodyIDs_.insert("root", 0);
51  joints_.append(new joints::null(*this));
52  XT_.append(spatialTransform());
53 
54  nDoF_ = 0;
55  unitQuaternions_ = false;
56 
57  resizeState();
58 }
59 
60 
61 void Foam::RBD::rigidBodyModel::resizeState()
62 {
63  Xlambda_.append(spatialTransform());
64  X0_.append(spatialTransform());
65 
66  v_.append(Zero);
67  a_.append(Zero);
68  c_.append(Zero);
69 
70  IA_.append(spatialTensor::I);
71  pA_.append(Zero);
72 
73  S_.append(Zero);
74  S1_.append(Zero);
75  U_.append(Zero);
76  U1_.append(Zero);
77  Dinv_.append(Zero);
78  u_.append(Zero);
79 }
80 
81 
82 void Foam::RBD::rigidBodyModel::addRestraints
83 (
84  const dictionary& dict
85 )
86 {
87  if (dict.found("restraints"))
88  {
89  const dictionary& restraintDict = dict.subDict("restraints");
90 
91  label i = 0;
92 
93  restraints_.setSize(restraintDict.size());
94 
95  forAllConstIter(IDLList<entry>, restraintDict, iter)
96  {
97  if (iter().isDict())
98  {
99  restraints_.set
100  (
101  i++,
103  (
104  iter().keyword(),
105  iter().dict(),
106  *this
107  )
108  );
109  }
110  }
111 
112  restraints_.setSize(i);
113  }
114 }
115 
116 
117 // * * * * * * * * * * * * Protected Member Functions * * * * * * * * * * * //
118 
120 (
121  const label parentID,
122  const spatialTransform& XT,
123  autoPtr<joint> jointPtr,
124  autoPtr<rigidBody> bodyPtr
125 )
126 {
127  // Append the body
128  const rigidBody& body = bodyPtr();
129  bodies_.append(bodyPtr);
130  const label bodyID = nBodies()-1;
131  bodyIDs_.insert(body.name(), bodyID);
132 
133  // If the parentID refers to a merged body find the parent into which it has
134  // been merged and set lambda and XT accordingly
135  if (merged(parentID))
136  {
137  const subBody& sBody = mergedBody(parentID);
138  lambda_.append(sBody.masterID());
139  XT_.append(XT & sBody.masterXT());
140  }
141  else
142  {
143  lambda_.append(parentID);
144  XT_.append(XT);
145  }
146 
147  // Append the joint
148  const joint& prevJoint = joints_[joints_.size() - 1];
149  joints_.append(jointPtr);
150  joint& curJoint = joints_[joints_.size() - 1];
151  curJoint.index() = joints_.size() - 1;
152  curJoint.qIndex() = prevJoint.qIndex() + prevJoint.nDoF();
153 
154  // Increment the degrees of freedom
155  nDoF_ += curJoint.nDoF();
156  unitQuaternions_ = unitQuaternions_ || curJoint.unitQuaternion();
157 
158  resizeState();
159 
160  return bodyID;
161 }
162 
163 
164 // * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * * //
165 
167 :
168  g_(Zero)
169 {
170  initialiseRootBody();
171 }
172 
173 
175 :
176  g_(Zero)
177 {
178  initialiseRootBody();
179 
180  const dictionary& bodiesDict = dict.subDict("bodies");
181 
182  forAllConstIter(IDLList<entry>, bodiesDict, iter)
183  {
184  const dictionary& bodyDict = iter().dict();
185 
186  if (bodyDict.found("mergeWith"))
187  {
188  merge
189  (
190  bodyID(bodyDict.lookup("mergeWith")),
191  bodyDict.lookup("transform"),
192  rigidBody::New(iter().keyword(), bodyDict)
193  );
194  }
195  else
196  {
197  join
198  (
199  bodyID(bodyDict.lookup("parent")),
200  bodyDict.lookup("transform"),
201  joint::New(*this, bodyDict.subDict("joint")),
202  rigidBody::New(iter().keyword(), bodyDict)
203  );
204  }
205  }
206 
207  // Read the restraints and any other re-readable settings.
208  read(dict);
209 }
210 
211 
212 // * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
213 
215 {}
216 
217 
218 // * * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * //
219 
221 (
222  const label parentID,
223  const spatialTransform& XT,
224  autoPtr<joint> jointPtr,
225  autoPtr<rigidBody> bodyPtr
226 )
227 {
228  if (isA<joints::composite>(jointPtr()))
229  {
230  return join
231  (
232  parentID,
233  XT,
235  (
236  dynamic_cast<joints::composite*>(jointPtr.ptr())
237  ),
238  bodyPtr
239  );
240  }
241  else
242  {
243  return join_
244  (
245  parentID,
246  XT,
247  jointPtr,
248  bodyPtr
249  );
250  }
251 }
252 
253 
255 (
256  const label parentID,
257  const spatialTransform& XT,
258  autoPtr<joints::composite> cJointPtr,
259  autoPtr<rigidBody> bodyPtr
260 )
261 {
262  label parent = parentID;
263  joints::composite& cJoint = cJointPtr();
264 
265  // For all but the final joint in the set add a jointBody with the
266  // joint and transform
267  for (label j=0; j<cJoint.size()-1; j++)
268  {
269  parent = join_
270  (
271  parent,
272  j == 0 ? XT : spatialTransform(),
273  cJoint[j].clone(),
275  );
276  }
277 
278  // For the final joint in the set add the real body
279  parent = join_
280  (
281  parent,
282  cJoint.size() == 1 ? XT : spatialTransform(),
283  autoPtr<joint>(cJointPtr.ptr()),
284  bodyPtr
285  );
286 
287  // Set the properties of the last joint in the list to those set
288  // by rigidBodyModel
289  cJoint.setLastJoint();
290 
291  return parent;
292 }
293 
294 
295 void Foam::RBD::rigidBodyModel::makeComposite(const label bodyID)
296 {
297  if (!isA<compositeBody>(bodies_[bodyID]))
298  {
299  // Retrieve the un-merged body
300  autoPtr<rigidBody> bodyPtr = bodies_.set(bodyID, nullptr);
301 
302  // Insert the compositeBody containing the original body
303  bodies_.set
304  (
305  bodyID,
306  new compositeBody(bodyPtr)
307  );
308  }
309 }
310 
311 
313 (
314  const label parentID,
315  const spatialTransform& XT,
316  autoPtr<rigidBody> bodyPtr
317 )
318 {
319  autoPtr<subBody> sBodyPtr;
320 
321  // If the parentID refers to a merged body find the parent into which it has
322  // been merged and merge this on into the same parent with the appropriate
323  // transform
324  if (merged(parentID))
325  {
326  const subBody& sBody = mergedBody(parentID);
327 
328  makeComposite(sBody.masterID());
329 
330  sBodyPtr.set
331  (
332  new subBody
333  (
334  bodyPtr,
335  bodies_[sBody.masterID()].name(),
336  sBody.masterID(),
337  XT & sBody.masterXT()
338  )
339  );
340  }
341  else
342  {
343  makeComposite(parentID);
344 
345  sBodyPtr.set
346  (
347  new subBody
348  (
349  bodyPtr,
350  bodies_[parentID].name(),
351  parentID,
352  XT
353  )
354  );
355  }
356 
357  const subBody& sBody = sBodyPtr();
358  mergedBodies_.append(sBodyPtr);
359 
360  // Merge the sub-body with the parent
361  bodies_[sBody.masterID()].merge(sBody);
362 
363  const label sBodyID = mergedBodyID(mergedBodies_.size() - 1);
364  bodyIDs_.insert(sBody.name(), sBodyID);
365 
366  return sBodyID;
367 }
368 
369 
371 (
372  const label bodyId
373 ) const
374 {
375  if (merged(bodyId))
376  {
377  const subBody& mBody = mergedBody(bodyId);
378  return mBody.masterXT() & X0_[mBody.masterID()];
379  }
380  else
381  {
382  return X0_[bodyId];
383  }
384 }
385 
386 
388 {
389  wordList names(nBodies());
390 
391  label j = 0;
392  for (label i=1; i<nBodies(); i++)
393  {
394  if (!isType<jointBody>(bodies_[i]))
395  {
396  names[j++] = bodies_[i].name();
397  }
398  }
399 
400  names.setSize(j);
401 
402  return names;
403 }
404 
405 
407 {
408  os << indent << "bodies" << nl
410 
411  // Write the moving bodies
412  for (label i=1; i<nBodies(); i++)
413  {
414  // Do not write joint-bodies created automatically to support elements
415  // of composite joints
416  if (!isType<jointBody>(bodies_[i]))
417  {
418  os << indent << bodies_[i].name() << nl
420 
421  bodies_[i].write(os);
422 
423  writeEntry(os, "parent", bodies_[lambda_[i]].name());
424  writeEntry(os, "transform", XT_[i]);
425 
426  os << indent << "joint" << nl << joints_[i] << endl;
427  os << decrIndent << indent << token::END_BLOCK << endl;
428  }
429  }
430 
431  // Write the bodies merged into the parent bodies for efficiency
433  {
434  os << indent << mergedBodies_[i].name() << nl
436 
437  mergedBodies_[i].body().write(os);
438 
439  writeEntry(os, "transform", mergedBodies_[i].masterXT());
440 
441  writeEntry(os, "mergeWith", mergedBodies_[i].masterName());
442 
443  os << decrIndent << indent << token::END_BLOCK << endl;
444  }
445 
446  os << decrIndent << indent << token::END_BLOCK << nl;
447 
448 
449  if (!restraints_.empty())
450  {
451  os << indent << "restraints" << nl
453 
454  forAll(restraints_, ri)
455  {
456  word restraintType = restraints_[ri].type();
457 
458  os << indent << restraints_[ri].name() << nl
460 
461  restraints_[ri].write(os);
462 
463  os << decrIndent << indent << token::END_BLOCK << endl;
464  }
465 
466  os << decrIndent << indent << token::END_BLOCK << nl;
467  }
468 }
469 
470 
472 {
473  restraints_.clear();
474  addRestraints(dict);
475 
476  return true;
477 }
478 
479 
480 // * * * * * * * * * * * * * * * Ostream Operator * * * * * * * * * * * * * //
481 
483 {
484  rbm.write(os);
485  return os;
486 }
487 
488 
489 // ************************************************************************* //
Template class for intrusive linked lists.
Definition: ILList.H:50
const subBody & mergedBody(label mergedBodyID) const
Return the merged body for the given body ID.
Abstract base-class for all rigid-body joints.
Definition: joint.H:80
static autoPtr< rigidBody > New(const word &name, const scalar &m, const vector &c, const symmTensor &Ic)
Select constructed from components.
Definition: rigidBody.C:60
Ostream & operator<<(Ostream &, const rigidBody &)
Definition: rigidBodyI.H:73
dictionary dict
bool found(const word &, bool recursive=false, bool patternMatch=true) const
Search dictionary for given keyword.
Definition: dictionary.C:643
#define forAll(list, i)
Loop across all elements in list.
Definition: UList.H:434
intWM_LABEL_SIZE_t label
A label is an int32_t or int64_t as specified by the pre-processor macro WM_LABEL_SIZE.
Definition: label.H:59
Ostream & indent(Ostream &os)
Indent stream.
Definition: Ostream.H:221
DynamicList< spatialTransform > X0_
Transform for external forces to the bodies reference frame.
A list of keyword definitions, which are a keyword followed by any number of values (e...
Definition: dictionary.H:156
label merge(const label parentID, const spatialTransform &X, autoPtr< rigidBody > bodyPtr)
Merge the given body with transform X into the parent with ID.
const word & name() const
Return name.
Definition: rigidBodyI.H:65
Ostream & endl(Ostream &os)
Add newline and flush stream.
Definition: Ostream.H:251
static const SpatialTensor I
Identity matrix for square matrices.
Definition: SpatialTensor.H:80
T * ptr()
Return object pointer for reuse.
Definition: autoPtrI.H:90
label nBodies() const
Return the number of bodies in the model (bodies().size())
label mergedBodyID(const label mergedBodyIndex) const
Return the merged body ID for the given merged body index.
const word & name() const
Return the body name.
Definition: subBodyI.H:57
const dictionary & subDict(const word &) const
Find and return a sub-dictionary.
Definition: dictionary.C:982
HashTable< label, word > bodyIDs_
Lookup-table of the IDs of the bodies.
Prismatic joint for translation along the specified arbitrary axis.
PtrList< restraint > restraints_
Motion restraints.
PtrList< joint > joints_
Each body it attached with a joint which are held on this list.
T clone(const T &t)
Definition: List.H:54
PtrList< rigidBody > bodies_
List of the bodies.
This specialised rigidBody holds the original body after it has been merged into a master...
Definition: subBody.H:52
A class for handling words, derived from string.
Definition: word.H:59
const word & name(const label bodyID) const
Return the name of body with the given ID.
bool read(const dictionary &dict)
Read coefficients dictionary and update system parameters,.
DynamicList< spatialTransform > XT_
Transform from the parent body frame to the joint frame.
static const zero Zero
Definition: zero.H:97
This specialised rigidBody holds the original body after it has been merged into a parent...
Definition: compositeBody.H:52
defineTypeNameAndDebug(cuboid, 0)
virtual label join(const label parentID, const spatialTransform &XT, autoPtr< joint > jointPtr, autoPtr< rigidBody > bodyPtr)
Join the given body to the parent with ID parentID via the given.
forAllConstIter(PtrDictionary< phaseModel >, mixture.phases(), phase)
Definition: pEqn.H:29
virtual void write(Ostream &) const
Write.
An Ostream is an abstract base class for all output systems (streams, files, token lists...
Definition: Ostream.H:54
label nDoF() const
Return the number of degrees of freedom in this joint.
Definition: jointI.H:39
static const char nl
Definition: Ostream.H:260
DynamicList< label > lambda_
List of indices of the parent of each body.
label masterID() const
Return the master body Id.
Definition: subBodyI.H:69
void set(T *)
Set pointer to that given.
Definition: autoPtrI.H:99
Ostream & decrIndent(Ostream &os)
Decrement the indent level.
Definition: Ostream.H:235
void writeEntry(Ostream &os, const HashTable< T, Key, Hash > &ht)
Definition: HashTableIO.C:96
label size() const
Return the number of elements in the UPtrList.
Definition: UPtrListI.H:29
PtrList< subBody > mergedBodies_
Bodies may be merged into existing bodies, the inertia of which is.
void setSize(const label)
Reset size of List.
Definition: List.C:281
wordList movingBodyNames() const
Return the names of the moving bodies.
virtual ~rigidBodyModel()
Destructor.
static autoPtr< joint > New(joint *jointPtr)
Simple selector to return an autoPtr<joint> of the given joint*.
Definition: joint.C:43
Compact representation of the Plücker spatial transformation tensor in terms of the rotation tensor E...
An auto-pointer similar to the STL auto_ptr but with automatic casting to a reference to the type and...
Definition: PtrList.H:52
label bodyID(const word &name) const
Return the ID of the body with the given name.
Basic rigid-body model representing a system of rigid-bodies connected by 1-6 DoF joints...
spatialTransform X0(const label bodyId) const
Return the current transform to the global frame for the given body.
rigidBodyModel()
Null-constructor which adds the single root-body at the origin.
Ostream & incrIndent(Ostream &os)
Increment the indent level.
Definition: Ostream.H:228
virtual label join_(const label parentID, const spatialTransform &XT, autoPtr< joint > jointPtr, autoPtr< rigidBody > bodyPtr)
Join the given body to the parent with ID parentID via the given.
virtual bool unitQuaternion() const
Return true if this joint describes rotation using a quaternion.
Definition: jointI.H:44
bool merged(label bodyID) const
Return true if the body with given ID has been merged with a parent.
static autoPtr< restraint > New(const word &name, const dictionary &dict, const rigidBodyModel &model)
Select constructed from the dict dictionary and Time.
vector g_
Acceleration due to gravity.
Namespace for OpenFOAM.
ITstream & lookup(const word &, bool recursive=false, bool patternMatch=true) const
Find and return an entry data stream.
Definition: dictionary.C:844
const spatialTransform & masterXT() const
Return the transform with respect to the master body.
Definition: subBodyI.H:75