rigidBodyMotion.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 "rigidBodyMotion.H"
27 #include "rigidBodySolver.H"
28 #include "septernion.H"
29 
30 // * * * * * * * * * * * * * Private Member Functions * * * * * * * * * * * //
31 
32 void Foam::RBD::rigidBodyMotion::initialize()
33 {
34  // Calculate the initial body-state
35  forwardDynamicsCorrection(rigidBodyModelState(*this));
36  X00_ = X0_;
37 
38  // Update the body-state to correspond to the current joint-state
39  forwardDynamicsCorrection(motionState_);
40 }
41 
42 
43 // * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
44 
46 :
48  motionState_(*this),
49  motionState0_(*this),
50  aRelax_(1.0),
51  aDamp_(1.0),
52  report_(false),
53  solver_(nullptr)
54 {}
55 
57 (
58  const dictionary& dict
59 )
60 :
61  rigidBodyModel(dict),
62  motionState_(*this, dict),
63  motionState0_(motionState_),
64  X00_(X0_.size()),
65  aRelax_(dict.lookupOrDefault<scalar>("accelerationRelaxation", 1.0)),
66  aDamp_(dict.lookupOrDefault<scalar>("accelerationDamping", 1.0)),
67  report_(dict.lookupOrDefault<Switch>("report", false)),
68  solver_(rigidBodySolver::New(*this, dict.subDict("solver")))
69 {
70  if (dict.found("g"))
71  {
72  g() = vector(dict.lookup("g"));
73  }
74 
75  initialize();
76 }
77 
78 
80 (
81  const dictionary& dict,
82  const dictionary& stateDict
83 )
84 :
85  rigidBodyModel(dict),
86  motionState_(*this, stateDict),
87  motionState0_(motionState_),
88  X00_(X0_.size()),
89  aRelax_(dict.lookupOrDefault<scalar>("accelerationRelaxation", 1.0)),
90  aDamp_(dict.lookupOrDefault<scalar>("accelerationDamping", 1.0)),
91  report_(dict.lookupOrDefault<Switch>("report", false)),
92  solver_(rigidBodySolver::New(*this, dict.subDict("solver")))
93 {
94  if (dict.found("g"))
95  {
96  g() = vector(dict.lookup("g"));
97  }
98 
99  initialize();
100 }
101 
102 
103 // * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
104 
106 {}
107 
108 
109 // * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
110 
112 (
113  const label bodyId
114 ) const
115 {
116  if (merged(bodyId))
117  {
118  const subBody& mBody = mergedBody(bodyId);
119  return mBody.masterXT() & X00_[mBody.masterID()];
120  }
121  else
122  {
123  return X00_[bodyId];
124  }
125 }
126 
127 
129 (
130  rigidBodyModelState& state,
131  const scalarField& tau,
132  const Field<spatialVector>& fx
133 ) const
134 {
135  scalarField qDdotPrev = state.qDdot();
136  rigidBodyModel::forwardDynamics(state, tau, fx);
137  state.qDdot() = aDamp_*(aRelax_*state.qDdot() + (1 - aRelax_)*qDdotPrev);
138 }
139 
140 
142 (
143  scalar deltaT,
144  const scalarField& tau,
145  const Field<spatialVector>& fx
146 )
147 {
148  motionState_.deltaT() = deltaT;
149 
150  if (motionState0_.deltaT() < SMALL)
151  {
152  motionState0_.deltaT() = deltaT;
153  }
154 
155  if (Pstream::master())
156  {
157  solver_->solve(tau, fx);
158  }
159 
160  Pstream::scatter(motionState_);
161 
162  // Update the body-state to correspond to the current joint-state
163  forwardDynamicsCorrection(motionState_);
164 }
165 
166 
168 {
169  const spatialTransform CofR(X0(bodyID));
170  const spatialVector vCofR(v(bodyID, Zero));
171 
172  Info<< "Rigid-body motion of the " << name(bodyID) << nl
173  << " Centre of rotation: " << CofR.r() << nl
174  << " Orientation: " << CofR.E() << nl
175  << " Linear velocity: " << vCofR.l() << nl
176  << " Angular velocity: " << vCofR.w()
177  << endl;
178 }
179 
180 
182 (
183  const label bodyID,
184  const pointField& initialPoints
185 ) const
186 {
187  // Calculate the transform from the initial state in the global frame
188  // to the current state in the global frame
189  spatialTransform X(X0(bodyID).inv() & X00(bodyID));
190 
191  tmp<pointField> tpoints(new pointField(initialPoints.size()));
192  pointField& points = tpoints.ref();
193 
194  forAll(points, i)
195  {
196  points[i] = X.transformPoint(initialPoints[i]);
197  }
198 
199  return tpoints;
200 }
201 
202 
204 (
205  const label bodyID,
206  const scalarField& weight,
207  const pointField& initialPoints
208 ) const
209 {
210  // Calculate the transform from the initial state in the global frame
211  // to the current state in the global frame
212  spatialTransform X(X0(bodyID).inv() & X00(bodyID));
213 
214  // Calculate the septernion equivalent of the transformation for 'slerp'
215  // interpolation
216  septernion s(X);
217 
218  tmp<pointField> tpoints(new pointField(initialPoints));
219  pointField& points = tpoints.ref();
220 
221  forAll(points, i)
222  {
223  // Move non-stationary points
224  if (weight[i] > SMALL)
225  {
226  // Use solid-body motion where weight = 1
227  if (weight[i] > 1 - SMALL)
228  {
229  points[i] = X.transformPoint(initialPoints[i]);
230  }
231  // Slerp septernion interpolation
232  else
233  {
234  points[i] =
235  slerp(septernion::I, s, weight[i])
236  .transformPoint(initialPoints[i]);
237  }
238  }
239  }
240 
241  return tpoints;
242 }
243 
244 
246 (
247  const labelList& bodyIDs,
248  const List<const scalarField*>& weights,
249  const pointField& initialPoints
250 ) const
251 {
252  List<septernion> ss(bodyIDs.size() + 1);
253  ss[bodyIDs.size()] = septernion::I;
254 
255  forAll(bodyIDs, bi)
256  {
257  const label bodyID = bodyIDs[bi];
258 
259  // Calculate the transform from the initial state in the global frame
260  // to the current state in the global frame
261  spatialTransform X(X0(bodyID).inv() & X00(bodyID));
262 
263  // Calculate the septernion equivalent of the transformation
264  ss[bi] = septernion(X);
265  }
266 
267  tmp<pointField> tpoints(new pointField(initialPoints));
268  pointField& points = tpoints.ref();
269 
270  List<scalar> w(ss.size());
271 
272  forAll(points, i)
273  {
274  // Initialize to 1 for the far-field weight
275  scalar sum1mw = 1;
276 
277  forAll(bodyIDs, bi)
278  {
279  w[bi] = (*(weights[bi]))[i];
280  sum1mw += w[bi]/(1 + SMALL - w[bi]);
281  }
282 
283  // Calculate the limiter for wi/(1 - wi) to ensure the sum(wi) = 1
284  scalar lambda = 1/sum1mw;
285 
286  // Limit wi/(1 - wi) and sum the resulting wi
287  scalar sumw = 0;
288  forAll(bodyIDs, bi)
289  {
290  w[bi] = lambda*w[bi]/(1 + SMALL - w[bi]);
291  sumw += w[bi];
292  }
293 
294  // Calculate the weight for the stationary far-field
295  w[bodyIDs.size()] = 1 - sumw;
296 
297  points[i] = average(ss, w).transformPoint(initialPoints[i]);
298  }
299 
300  return tpoints;
301 }
302 
303 
304 // ************************************************************************* //
static autoPtr< rigidBodySolver > New(rigidBodyMotion &body, const 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
void forwardDynamics(rigidBodyModelState &state, const scalarField &tau, const Field< spatialVector > &fx) const
Calculate the joint acceleration qDdot from the joint state q,.
DynamicList< spatialTransform > X0_
Transform for external forces to the bodies reference frame.
const vector & r() const
Return the translation vector.
Vector< Cmpt > l() const
Return the linear part of the spatial vector as a vector.
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.
dimensionedSphericalTensor inv(const dimensionedSphericalTensor &dt)
void status(const label bodyID) const
Report the status of the motion of the given body.
void size(const label)
Override size to be inconsistent with allocated storage.
Definition: ListI.H:76
const dictionary & subDict(const word &) const
Find and return a sub-dictionary.
Definition: dictionary.C:633
const DynamicList< label > & lambda() const
List of indices of the parent of each body.
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
bool merged(label bodyID) const
Return true if the body with given ID has been merged with a parent.
A simple wrapper around bool so that it can be read as a word: true/false, on/off, yes/no, y/n, t/f, or none.
Definition: Switch.H:60
static bool master(const label communicator=0)
Am I the master process.
Definition: UPstream.H:411
spatialTransform X00(const label bodyId) const
Return the initial transform to the global frame for the.
Vector< scalar > vector
A scalar version of the templated Vector.
Definition: vector.H:49
Septernion class used to perform translations and rotations in 3D space.
Definition: septernion.H:65
const tensor & E() const
Return the rotation tensor.
Holds the motion state of rigid-body model.
gmvFile<< "tracers "<< particles.size()<< nl;forAllConstIter(Cloud< passiveParticle >, particles, iter){gmvFile<< iter().position().x()<< " ";}gmvFile<< nl;forAllConstIter(Cloud< passiveParticle >, particles, iter){gmvFile<< iter().position().y()<< " ";}gmvFile<< nl;forAllConstIter(Cloud< passiveParticle >, particles, iter){gmvFile<< iter().position().z()<< " ";}gmvFile<< nl;forAll(lagrangianScalarNames, i){word name=lagrangianScalarNames[i];IOField< scalar > s(IOobject(name, runTime.timeName(), cloud::prefix, mesh, IOobject::MUST_READ, IOobject::NO_WRITE))
vectorField pointField
pointField is a vectorField.
Definition: pointFieldFwd.H:42
const pointField & points
const spatialVector & v(const label i) const
Return the spatial velocity of the bodies.
spatialTransform X0(const label bodyId) const
Return the current transform to the global frame for the given body.
void solve(scalar deltaT, const scalarField &tau, const Field< spatialVector > &fx)
Integrate velocities, orientation and position.
const vector & g() const
Return the acceleration due to gravity.
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
dimensioned< Type > average(const DimensionedField< Type, GeoMesh > &df)
static const septernion I
Definition: septernion.H:83
void forwardDynamics(rigidBodyModelState &state, const scalarField &tau, const Field< spatialVector > &fx) const
Calculate and optionally relax the joint acceleration qDdot from.
Vector< Cmpt > w() const
Return the angular part of the spatial vector as a vector.
static void scatter(const List< commsStruct > &comms, T &Value, const int tag, const label comm)
Scatter data. Distribute without modification. Reverse of gather.
tmp< pointField > transformPoints(const label bodyID, const pointField &initialPoints) const
Transform the given initial pointField of the specified body.
static const char nl
Definition: Ostream.H:262
rigidBodyMotion()
Construct null.
const spatialTransform & masterXT() const
Return the transform with respect to the master body.
Definition: subBodyI.H:75
scalar deltaT() const
Return access to the time-step.
quaternion slerp(const quaternion &qa, const quaternion &qb, const scalar t)
Spherical linear interpolation of quaternions.
Definition: quaternion.C:55
messageStream Info
Compact representation of the Plücker spatial transformation tensor in terms of the rotation tensor E...
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.
A class for managing temporary objects.
Definition: PtrList.H:54
vector transformPoint(const vector &p) const
Transform position p.
T & ref() const
Return non-const reference or generate a fatal error.
Definition: tmpI.H:174
rigidBodyModel()
Null-constructor which adds the single root-body at the origin.
void forwardDynamicsCorrection(const rigidBodyModelState &state) const
Correct the velocity and acceleration of the bodies in the model.
T lookupOrDefault(const word &, const T &, bool recursive=false, bool patternMatch=true) const
Find and return a T,.
const scalarField & qDdot() const
Return access to the joint acceleration.
ITstream & lookup(const word &, bool recursive=false, bool patternMatch=true) const
Find and return an entry data stream.
Definition: dictionary.C:451