rigidBodyModel.C
Go to the documentation of this file.
1 /*---------------------------------------------------------------------------*\
2  ========= |
3  \\ / F ield | OpenFOAM: The Open Source CFD Toolbox
4  \\ / O peration |
5  \\ / A nd | Copyright (C) 2016 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::initializeRootBody()
47 {
48  bodies_.append(new masslessBody("root"));
49  lambda_.append(0);
50  bodyIDs_.insert("root", 0);
51  joints_.append(new joints::null());
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 // * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * * //
118 
120 :
121  g_(Zero)
122 {
123  initializeRootBody();
124 }
125 
126 
128 :
129  g_(Zero)
130 {
131  initializeRootBody();
132 
133  const dictionary& bodiesDict = dict.subDict("bodies");
134 
135  forAllConstIter(IDLList<entry>, bodiesDict, iter)
136  {
137  const dictionary& bodyDict = iter().dict();
138 
139  if (bodyDict.found("mergeWith"))
140  {
141  merge
142  (
143  bodyID(bodyDict.lookup("mergeWith")),
144  bodyDict.lookup("transform"),
145  rigidBody::New(iter().keyword(), bodyDict)
146  );
147  }
148  else
149  {
150  join
151  (
152  bodyID(bodyDict.lookup("parent")),
153  bodyDict.lookup("transform"),
154  joint::New(bodyDict.subDict("joint")),
155  rigidBody::New(iter().keyword(), bodyDict)
156  );
157  }
158  }
159 
160  // Read the restraints and any other re-readable settings.
161  read(dict);
162 }
163 
164 
165 // * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
166 
168 {}
169 
170 
171 // * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
172 
174 (
175  const label parentID,
176  const spatialTransform& XT,
177  autoPtr<joint> jointPtr,
178  autoPtr<rigidBody> bodyPtr
179 )
180 {
181  // Append the body
182  const rigidBody& body = bodyPtr();
183  bodies_.append(bodyPtr);
184  const label bodyID = nBodies()-1;
185  bodyIDs_.insert(body.name(), bodyID);
186 
187  // If the parentID refers to a merged body find the parent into which it has
188  // been merged and set lambda and XT accordingly
189  if (merged(parentID))
190  {
191  const subBody& sBody = mergedBody(parentID);
192  lambda_.append(sBody.masterID());
193  XT_.append(XT & sBody.masterXT());
194  }
195  else
196  {
197  lambda_.append(parentID);
198  XT_.append(XT);
199  }
200 
201  // Append the joint
202  const joint& prevJoint = joints_[joints_.size() - 1];
203  joints_.append(jointPtr);
204  joint& curJoint = joints_[joints_.size() - 1];
205  curJoint.index() = joints_.size() - 1;
206  curJoint.qIndex() = prevJoint.qIndex() + prevJoint.nDoF();
207 
208  // Increment the degrees of freedom
209  nDoF_ += curJoint.nDoF();
211 
212  resizeState();
213 
214  return bodyID;
215 }
216 
217 
219 (
220  const label parentID,
221  const spatialTransform& XT,
222  autoPtr<joint> jointPtr,
223  autoPtr<rigidBody> bodyPtr
224 )
225 {
226  if (isA<joints::composite>(jointPtr()))
227  {
228  return join
229  (
230  parentID,
231  XT,
233  (
234  dynamic_cast<joints::composite*>(jointPtr.ptr())
235  ),
236  bodyPtr
237  );
238  }
239  else
240  {
241  return join_
242  (
243  parentID,
244  XT,
245  jointPtr,
246  bodyPtr
247  );
248  }
249 }
250 
251 
253 (
254  const label parentID,
255  const spatialTransform& XT,
256  autoPtr<joints::composite> cJointPtr,
257  autoPtr<rigidBody> bodyPtr
258 )
259 {
260  label parent = parentID;
261  joints::composite& cJoint = cJointPtr();
262 
263  // For all but the final joint in the set add a jointBody with the
264  // joint and transform
265  for (label j=0; j<cJoint.size()-1; j++)
266  {
267  parent = join_
268  (
269  parent,
270  j == 0 ? XT : spatialTransform(),
271  cJoint[j].clone(),
273  );
274  }
275 
276  // For the final joint in the set add the real body
277  parent = join_
278  (
279  parent,
280  cJoint.size() == 1 ? XT : spatialTransform(),
281  autoPtr<joint>(cJointPtr.ptr()),
282  bodyPtr
283  );
284 
285  // Set the properties of the last joint in the list to those set
286  // by rigidBodyModel
287  cJoint.setLastJoint();
288 
289  return parent;
290 }
291 
292 
293 void Foam::RBD::rigidBodyModel::makeComposite(const label bodyID)
294 {
295  if (!isA<compositeBody>(bodies_[bodyID]))
296  {
297  // Retrieve the un-merged body
298  autoPtr<rigidBody> bodyPtr = bodies_.set(bodyID, NULL);
299 
300  // Insert the compositeBody containing the original body
301  bodies_.set
302  (
303  bodyID,
304  new compositeBody(bodyPtr)
305  );
306  }
307 }
308 
309 
311 (
312  const label parentID,
313  const spatialTransform& XT,
314  autoPtr<rigidBody> bodyPtr
315 )
316 {
317  autoPtr<subBody> sBodyPtr;
318 
319  // If the parentID refers to a merged body find the parent into which it has
320  // been merged and merge this on into the same parent with the appropriate
321  // transform
322  if (merged(parentID))
323  {
324  const subBody& sBody = mergedBody(parentID);
325 
326  makeComposite(sBody.masterID());
327 
328  sBodyPtr.set
329  (
330  new subBody
331  (
332  bodyPtr,
333  bodies_[sBody.masterID()].name(),
334  sBody.masterID(),
335  XT & sBody.masterXT()
336  )
337  );
338  }
339  else
340  {
341  makeComposite(parentID);
342 
343  sBodyPtr.set
344  (
345  new subBody
346  (
347  bodyPtr,
348  bodies_[parentID].name(),
349  parentID,
350  XT
351  )
352  );
353  }
354 
355  const subBody& sBody = sBodyPtr();
356  mergedBodies_.append(sBodyPtr);
357 
358  // Merge the sub-body with the parent
359  bodies_[sBody.masterID()].merge(sBody);
360 
361  const label sBodyID = mergedBodyID(mergedBodies_.size() - 1);
362  bodyIDs_.insert(sBody.name(), sBodyID);
363 
364  return sBodyID;
365 }
366 
367 
369 (
370  const label bodyId
371 ) const
372 {
373  if (merged(bodyId))
374  {
375  const subBody& mBody = mergedBody(bodyId);
376  return mBody.masterXT() & X0_[mBody.masterID()];
377  }
378  else
379  {
380  return X0_[bodyId];
381  }
382 }
383 
384 
386 {
387  os << indent << "bodies" << nl
389 
390  // Write the moving bodies
391  for (label i=1; i<nBodies(); i++)
392  {
393  // Do not write joint-bodies created automatically to support elements
394  // of composite joints
395  if (!isType<jointBody>(bodies_[i]))
396  {
397  os << indent << bodies_[i].name() << nl
399 
400  bodies_[i].write(os);
401 
402  os.writeKeyword("parent")
403  << bodies_[lambda_[i]].name() << token::END_STATEMENT << nl;
404 
405  os.writeKeyword("transform")
406  << XT_[i] << token::END_STATEMENT << nl;
407 
408  os << indent << "joint" << nl << joints_[i] << endl;
409 
410  os << decrIndent << indent << token::END_BLOCK << endl;
411  }
412  }
413 
414  // Write the bodies merged into the parent bodies for efficiency
416  {
417  os << indent << mergedBodies_[i].name() << nl
419 
420  mergedBodies_[i].body().write(os);
421 
422  os.writeKeyword("transform")
423  << mergedBodies_[i].masterXT() << token::END_STATEMENT << nl;
424 
425  os.writeKeyword("mergeWith")
426  << mergedBodies_[i].masterName() << token::END_STATEMENT << nl;
427 
428  os << decrIndent << indent << token::END_BLOCK << endl;
429  }
430 
431  os << decrIndent << indent << token::END_BLOCK << nl;
432 
433 
434  if (!restraints_.empty())
435  {
436  os << indent << "restraints" << nl
438 
439  forAll(restraints_, ri)
440  {
441  word restraintType = restraints_[ri].type();
442 
443  os << indent << restraints_[ri].name() << nl
445 
446  restraints_[ri].write(os);
447 
448  os << decrIndent << indent << token::END_BLOCK << endl;
449  }
450 
451  os << decrIndent << indent << token::END_BLOCK << nl;
452  }
453 }
454 
455 
457 {
458  restraints_.clear();
459  addRestraints(dict);
460 
461  return true;
462 }
463 
464 
465 // * * * * * * * * * * * * * * * Ostream Operator * * * * * * * * * * * * * //
466 
468 {
469  rbm.write(os);
470  return os;
471 }
472 
473 
474 // ************************************************************************* //
Abstract base-class for all rigid-body joints.
Definition: joint.H:79
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
void append(T *)
Append an element at the end of the list.
Definition: PtrListI.H:39
dictionary dict
#define forAll(list, i)
Loop across all elements in list.
Definition: UList.H:428
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:223
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:137
const subBody & mergedBody(label mergedBodyID) const
Return the merged body for the given body ID.
label bodyID(const word &name) const
Return the ID of the body with the given name.
label merge(const label parentID, const spatialTransform &X, autoPtr< rigidBody > bodyPtr)
Merge the given body with transform X into the parent with ID.
const dictionary & subDict(const word &) const
Find and return a sub-dictionary.
Definition: dictionary.C:633
label masterID() const
Return the master body Id.
Definition: subBodyI.H:69
Ostream & endl(Ostream &os)
Add newline and flush stream.
Definition: Ostream.H:253
const word & name() const
Return name.
Definition: rigidBodyI.H:65
bool merged(label bodyID) const
Return true if the body with given ID has been merged with a parent.
static const SpatialTensor I
Identity matrix for square matrices.
Definition: SpatialTensor.H:80
T * ptr()
Return object pointer for reuse.
Definition: autoPtrI.H:90
const word & name() const
Return the body name.
Definition: subBodyI.H:57
label nBodies() const
Return the number of bodies in the model (bodies().size())
HashTable< label, word > bodyIDs_
Lookup-table of the IDs of the bodies.
virtual bool unitQuaternion() const
Return true if this joint describes rotation using a quaternion.
Definition: jointI.H:43
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.
PtrList< rigidBody > bodies_
List of the bodies.
A class for handling words, derived from string.
Definition: word.H:59
spatialTransform X0(const label bodyId) const
Return the current transform to the global frame for the given body.
label nDoF_
The number of degrees of freedom of the model.
DynamicList< T, SizeInc, SizeMult, SizeDiv > & append(const T &)
Append an element at the end of the list.
Definition: DynamicListI.H:292
label nDoF() const
Return the number of degrees of freedom in this joint.
Definition: jointI.H:38
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.
bool found(const word &, bool recursive=false, bool patternMatch=true) const
Search dictionary for given keyword.
Definition: dictionary.C:306
static const zero Zero
Definition: zero.H:91
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
An Ostream is an abstract base class for all output systems (streams, files, token lists...
Definition: Ostream.H:53
static const char nl
Definition: Ostream.H:262
virtual void write(Ostream &) const
Write.
DynamicList< label > lambda_
List of indices of the parent of each body.
void set(T *)
Set pointer to that given.
Definition: autoPtrI.H:99
Ostream & decrIndent(Ostream &os)
Decrement the indent level.
Definition: Ostream.H:237
Ostream & writeKeyword(const keyType &)
Write the keyword followed by an appropriate indentation.
Definition: Ostream.C:54
const spatialTransform & masterXT() const
Return the transform with respect to the master body.
Definition: subBodyI.H:75
bool unitQuaternions_
True if any of the joints using quaternions.
label mergedBodyID(const label mergedBodyIndex) const
Return the merged body ID for the given merged body index.
PtrList< subBody > mergedBodies_
Bodies may be merged into existing bodies, the inertia of which is.
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:53
Basic rigid-body model representing a system of rigid-bodies connected by 1-6 DoF joints...
const word & name(const label bodyID) const
Return the name of body with the given ID.
rigidBodyModel()
Null-constructor which adds the single root-body at the origin.
Ostream & incrIndent(Ostream &os)
Increment the indent level.
Definition: Ostream.H:230
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.
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.
label size() const
Return the number of elements in the UPtrList.
Definition: UPtrListI.H:29
ITstream & lookup(const word &, bool recursive=false, bool patternMatch=true) const
Find and return an entry data stream.
Definition: dictionary.C:451