rigidBodyMotion.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 "rigidBodyMotion.H"
27 #include "rigidBodySolver.H"
28 #include "septernion.H"
29 
30 // * * * * * * * * * * * * * Private Member Functions * * * * * * * * * * * //
31 
32 void Foam::RBD::rigidBodyMotion::initialise()
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  initialise();
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  initialise();
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  const scalar t,
144  const scalar deltaT,
145  const scalarField& tau,
146  const Field<spatialVector>& fx
147 )
148 {
149  motionState_.t() = t;
150  motionState_.deltaT() = deltaT;
151 
152  if (motionState0_.deltaT() < small)
153  {
154  motionState0_.t() = t;
155  motionState0_.deltaT() = deltaT;
156  }
157 
158  if (Pstream::master())
159  {
160  solver_->solve(tau, fx);
161  }
162 
163  Pstream::scatter(motionState_);
164 
165  // Update the body-state to correspond to the current joint-state
166  forwardDynamicsCorrection(motionState_);
167 }
168 
169 
171 {
172  const spatialTransform CofR(X0(bodyID));
173  const spatialVector vCofR(v(bodyID, Zero));
174 
175  Info<< "Rigid-body motion of the " << name(bodyID) << nl
176  << " Centre of rotation: " << CofR.r() << nl
177  << " Orientation: " << CofR.E() << nl
178  << " Linear velocity: " << vCofR.l() << nl
179  << " Angular velocity: " << vCofR.w()
180  << endl;
181 }
182 
183 
185 (
186  const label bodyID,
187  const pointField& initialPoints
188 ) const
189 {
190  // Calculate the transform from the initial state in the global frame
191  // to the current state in the global frame
192  spatialTransform X(X0(bodyID).inv() & X00(bodyID));
193 
194  tmp<pointField> tpoints(new pointField(initialPoints.size()));
195  pointField& points = tpoints.ref();
196 
197  forAll(points, i)
198  {
199  points[i] = X.transformPoint(initialPoints[i]);
200  }
201 
202  return tpoints;
203 }
204 
205 
207 (
208  const label bodyID,
209  const scalarField& weight,
210  const pointField& initialPoints
211 ) const
212 {
213  // Calculate the transform from the initial state in the global frame
214  // to the current state in the global frame
215  spatialTransform X(X0(bodyID).inv() & X00(bodyID));
216 
217  // Calculate the septernion equivalent of the transformation for 'slerp'
218  // interpolation
219  septernion s(X);
220 
221  tmp<pointField> tpoints(new pointField(initialPoints));
222  pointField& points = tpoints.ref();
223 
224  forAll(points, i)
225  {
226  // Move non-stationary points
227  if (weight[i] > small)
228  {
229  // Use solid-body motion where weight = 1
230  if (weight[i] > 1 - small)
231  {
232  points[i] = X.transformPoint(initialPoints[i]);
233  }
234  // Slerp septernion interpolation
235  else
236  {
237  points[i] =
238  slerp(septernion::I, s, weight[i])
239  .transformPoint(initialPoints[i]);
240  }
241  }
242  }
243 
244  return tpoints;
245 }
246 
247 
249 (
250  const labelList& bodyIDs,
251  const List<const scalarField*>& weights,
252  const pointField& initialPoints
253 ) const
254 {
255  List<septernion> ss(bodyIDs.size() + 1);
256  ss[bodyIDs.size()] = septernion::I;
257 
258  forAll(bodyIDs, bi)
259  {
260  const label bodyID = bodyIDs[bi];
261 
262  // Calculate the transform from the initial state in the global frame
263  // to the current state in the global frame
264  spatialTransform X(X0(bodyID).inv() & X00(bodyID));
265 
266  // Calculate the septernion equivalent of the transformation
267  ss[bi] = septernion(X);
268  }
269 
270  tmp<pointField> tpoints(new pointField(initialPoints));
271  pointField& points = tpoints.ref();
272 
273  List<scalar> w(ss.size());
274 
275  forAll(points, i)
276  {
277  // Initialise to 1 for the far-field weight
278  scalar sum1mw = 1;
279 
280  forAll(bodyIDs, bi)
281  {
282  w[bi] = (*(weights[bi]))[i];
283  sum1mw += w[bi]/(1 + small - w[bi]);
284  }
285 
286  // Calculate the limiter for wi/(1 - wi) to ensure the sum(wi) = 1
287  scalar lambda = 1/sum1mw;
288 
289  // Limit wi/(1 - wi) and sum the resulting wi
290  scalar sumw = 0;
291  forAll(bodyIDs, bi)
292  {
293  w[bi] = lambda*w[bi]/(1 + small - w[bi]);
294  sumw += w[bi];
295  }
296 
297  // Calculate the weight for the stationary far-field
298  w[bodyIDs.size()] = 1 - sumw;
299 
300  points[i] = average(ss, w).transformPoint(initialPoints[i]);
301  }
302 
303  return tpoints;
304 }
305 
306 
307 // ************************************************************************* //
const subBody & mergedBody(label mergedBodyID) const
Return the merged body for the given body ID.
bool found(const word &, bool recursive=false, bool patternMatch=true) const
Search dictionary for given keyword.
Definition: dictionary.C:643
static autoPtr< rigidBodySolver > New(rigidBodyMotion &body, const dictionary &dict)
#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
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
void forwardDynamicsCorrection(const rigidBodyModelState &state) const
Correct the velocity and acceleration of the bodies in the model.
tmp< pointField > transformPoints(const label bodyID, const pointField &initialPoints) const
Transform the given initial pointField of the specified body.
const spatialVector & v(const label i) const
Return the spatial velocity of the bodies.
T & ref() const
Return non-const reference or generate a fatal error.
Definition: tmpI.H:181
dimensionedSphericalTensor inv(const dimensionedSphericalTensor &dt)
void size(const label)
Override size to be inconsistent with allocated storage.
Definition: ListI.H:164
Ostream & endl(Ostream &os)
Add newline and flush stream.
Definition: Ostream.H:251
void status(const label bodyID) const
Report the status of the motion of the given body.
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/any.
Definition: Switch.H:60
static bool master(const label communicator=0)
Am I the master process.
Definition: UPstream.H:423
vector transformPoint(const vector &p) const
Transform position p.
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 vector & r() const
Return the translation vector.
const dictionary & subDict(const word &) const
Find and return a sub-dictionary.
Definition: dictionary.C:982
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
scalar t() const
Return access to the time.
This specialised rigidBody holds the original body after it has been merged into a master...
Definition: subBody.H:52
const word & name(const label bodyID) const
Return the name of body with the given ID.
static const zero Zero
Definition: zero.H:97
dimensioned< Type > average(const DimensionedField< Type, GeoMesh > &df)
static const septernion I
Definition: septernion.H:83
static void scatter(const List< commsStruct > &comms, T &Value, const int tag, const label comm)
Scatter data. Distribute without modification. Reverse of gather.
Vector< Cmpt > l() const
Return the linear part of the spatial vector as a vector.
static const char nl
Definition: Ostream.H:260
void solve(const scalar t, const scalar deltaT, const scalarField &tau, const Field< spatialVector > &fx)
Integrate velocities, orientation and position.
scalar deltaT() const
Return access to the time-step.
label masterID() const
Return the master body Id.
Definition: subBodyI.H:69
rigidBodyMotion()
Construct null.
const vector & g() const
Return the acceleration due to gravity.
Vector< Cmpt > w() const
Return the angular part of the spatial vector as a vector.
const tensor & E() const
Return the rotation tensor.
T lookupOrDefault(const word &, const T &, bool recursive=false, bool patternMatch=true) const
Find and return a T,.
quaternion slerp(const quaternion &qa, const quaternion &qb, const scalar t)
Spherical linear interpolation of quaternions.
Definition: quaternion.C:55
messageStream Info
const DynamicList< label > & lambda() const
List of indices of the parent of each body.
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...
A class for managing temporary objects.
Definition: PtrList.H:53
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.
const scalarField & qDdot() const
Return access to the joint acceleration.
void forwardDynamics(rigidBodyModelState &state, const scalarField &tau, const Field< spatialVector > &fx) const
Calculate the joint acceleration qDdot from the joint state q,.
bool merged(label bodyID) const
Return true if the body with given ID has been merged with a parent.
void forwardDynamics(rigidBodyModelState &state, const scalarField &tau, const Field< spatialVector > &fx) const
Calculate and optionally relax the joint acceleration qDdot from.
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