FreeFOAM The Cross-Platform CFD Toolkit
sixDoFRigidBodyMotion.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) 2009-2010 OpenCFD Ltd.
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 PARTICLUAR 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 "sixDoFRigidBodyMotion.H"
27 
28 // * * * * * * * * * * * * * Private Member Functions * * * * * * * * * * * //
29 
30 void Foam::sixDoFRigidBodyMotion::applyRestraints()
31 {
32  if (restraints_.empty())
33  {
34  return;
35  }
36 
37  if (Pstream::master())
38  {
39  forAll(restraints_, rI)
40  {
41  if (report_)
42  {
43  Info<< "Restraint " << restraintNames_[rI] << ": ";
44  }
45 
46  // restraint position
47  point rP = vector::zero;
48 
49  // restraint force
50  vector rF = vector::zero;
51 
52  // restraint moment
53  vector rM = vector::zero;
54 
55  restraints_[rI].restrain(*this, rP, rF, rM);
56 
57  a() += rF/mass_;
58 
59  // Moments are returned in global axes, transforming to
60  // body local to add to torque.
61  tau() += Q().T() & (rM + ((rP - centreOfMass()) ^ rF));
62  }
63  }
64 }
65 
66 
67 void Foam::sixDoFRigidBodyMotion::applyConstraints(scalar deltaT)
68 {
69  if (constraints_.empty())
70  {
71  return;
72  }
73 
74  if (Pstream::master())
75  {
76  label iteration = 0;
77 
78  bool allConverged = true;
79 
80  // constraint force accumulator
81  vector cFA = vector::zero;
82 
83  // constraint moment accumulator
84  vector cMA = vector::zero;
85 
86  do
87  {
88  allConverged = true;
89 
90  forAll(constraints_, cI)
91  {
92  if (sixDoFRigidBodyMotionConstraint::debug)
93  {
94  Info<< "Constraint " << constraintNames_[cI] << ": ";
95  }
96 
97  // constraint position
98  point cP = vector::zero;
99 
100  // constraint force
101  vector cF = vector::zero;
102 
103  // constraint moment
104  vector cM = vector::zero;
105 
106  bool constraintConverged = constraints_[cI].constrain
107  (
108  *this,
109  cFA,
110  cMA,
111  deltaT,
112  cP,
113  cF,
114  cM
115  );
116 
117  allConverged = allConverged && constraintConverged;
118 
119  // Accumulate constraint force
120  cFA += cF;
121 
122  // Accumulate constraint moment
123  cMA += cM + ((cP - centreOfMass()) ^ cF);
124  }
125 
126  } while(++iteration < maxConstraintIterations_ && !allConverged);
127 
128  if (iteration >= maxConstraintIterations_)
129  {
131  (
132  "Foam::sixDoFRigidBodyMotion::applyConstraints"
133  "(scalar deltaT)"
134  )
135  << nl << "Maximum number of sixDoFRigidBodyMotion constraint "
136  << "iterations ("
137  << maxConstraintIterations_
138  << ") exceeded." << nl
139  << exit(FatalError);
140  }
141 
142  Info<< "sixDoFRigidBodyMotion constraints converged in "
143  << iteration << " iterations" << endl;
144 
145  if (report_)
146  {
147  Info<< "Constraint force: " << cFA << nl
148  << "Constraint moment: " << cMA
149  << endl;
150  }
151 
152  // Add the constraint forces and moments to the motion state variables
153  a() += cFA/mass_;
154 
155  // The moment of constraint forces has already been added
156  // during accumulation. Moments are returned in global axes,
157  // transforming to body local
158  tau() += Q().T() & cMA;
159  }
160 }
161 
162 
163 // * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
164 
166 :
167  motionState_(),
168  restraints_(),
169  restraintNames_(),
170  constraints_(),
171  constraintNames_(),
172  maxConstraintIterations_(0),
173  initialCentreOfMass_(vector::zero),
174  initialQ_(I),
175  momentOfInertia_(diagTensor::one*VSMALL),
176  mass_(VSMALL),
177  cDamp_(0.0),
178  aLim_(VGREAT),
179  report_(false)
180 {}
181 
182 
184 (
185  const point& centreOfMass,
186  const tensor& Q,
187  const vector& v,
188  const vector& a,
189  const vector& pi,
190  const vector& tau,
191  scalar mass,
192  const point& initialCentreOfMass,
193  const tensor& initialQ,
194  const diagTensor& momentOfInertia,
195  scalar cDamp,
196  scalar aLim,
197  bool report
198 )
199 :
200  motionState_
201  (
202  centreOfMass,
203  Q,
204  v,
205  a,
206  pi,
207  tau
208  ),
209  restraints_(),
210  restraintNames_(),
211  constraints_(),
212  constraintNames_(),
213  maxConstraintIterations_(0),
214  initialCentreOfMass_(initialCentreOfMass),
215  initialQ_(initialQ),
216  momentOfInertia_(momentOfInertia),
217  mass_(mass),
218  cDamp_(cDamp),
219  aLim_(aLim),
220  report_(report)
221 {}
222 
223 
225 :
226  motionState_(dict),
227  restraints_(),
228  restraintNames_(),
229  constraints_(),
230  constraintNames_(),
231  maxConstraintIterations_(0),
232  initialCentreOfMass_
233  (
234  dict.lookupOrDefault("initialCentreOfMass", centreOfMass())
235  ),
236  initialQ_
237  (
238  dict.lookupOrDefault("initialOrientation", Q())
239  ),
240  momentOfInertia_(dict.lookup("momentOfInertia")),
241  mass_(readScalar(dict.lookup("mass"))),
242  cDamp_(dict.lookupOrDefault<scalar>("accelerationDampingCoeff", 0.0)),
243  aLim_(dict.lookupOrDefault<scalar>("accelerationLimit", VGREAT)),
244  report_(dict.lookupOrDefault<Switch>("report", false))
245 {
246  addRestraints(dict);
247 
248  addConstraints(dict);
249 }
250 
251 
253 (
254  const sixDoFRigidBodyMotion& sDoFRBM
255 )
256 :
257  motionState_(sDoFRBM.motionState_),
258  restraints_(sDoFRBM.restraints_),
259  restraintNames_(sDoFRBM.restraintNames_),
260  constraints_(sDoFRBM.constraints_),
261  constraintNames_(sDoFRBM.constraintNames_),
262  maxConstraintIterations_(sDoFRBM.maxConstraintIterations_),
263  initialCentreOfMass_(sDoFRBM.initialCentreOfMass_),
264  initialQ_(sDoFRBM.initialQ_),
265  momentOfInertia_(sDoFRBM.momentOfInertia_),
266  mass_(sDoFRBM.mass_),
267  cDamp_(sDoFRBM.cDamp_),
268  aLim_(sDoFRBM.aLim_),
269  report_(sDoFRBM.report_)
270 {}
271 
272 
273 // * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
274 
276 {}
277 
278 
279 // * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
280 
282 (
283  const dictionary& dict
284 )
285 {
286  if (dict.found("restraints"))
287  {
288  const dictionary& restraintDict = dict.subDict("restraints");
289 
290  label i = 0;
291 
292  restraints_.setSize(restraintDict.size());
293 
294  restraintNames_.setSize(restraintDict.size());
295 
296  forAllConstIter(IDLList<entry>, restraintDict, iter)
297  {
298  if (iter().isDict())
299  {
300  // Info<< "Adding restraint: " << iter().keyword() << endl;
301 
302  restraints_.set
303  (
304  i,
306  );
307 
308  restraintNames_[i] = iter().keyword();
309 
310  i++;
311  }
312  }
313 
314  restraints_.setSize(i);
315 
316  restraintNames_.setSize(i);
317  }
318 }
319 
320 
322 (
323  const dictionary& dict
324 )
325 {
326  if (dict.found("constraints"))
327  {
328  const dictionary& constraintDict = dict.subDict("constraints");
329 
330  label i = 0;
331 
332  constraints_.setSize(constraintDict.size());
333 
334  constraintNames_.setSize(constraintDict.size());
335 
336  forAllConstIter(IDLList<entry>, constraintDict, iter)
337  {
338  if (iter().isDict())
339  {
340  // Info<< "Adding constraint: " << iter().keyword() << endl;
341 
342  constraints_.set
343  (
344  i,
346  );
347 
348  constraintNames_[i] = iter().keyword();
349 
350  i++;
351  }
352  }
353 
354  constraints_.setSize(i);
355 
356  constraintNames_.setSize(i);
357 
358  if (!constraints_.empty())
359  {
360  maxConstraintIterations_ = readLabel
361  (
362  constraintDict.lookup("maxIterations")
363  );
364  }
365  }
366 }
367 
368 
370 (
371  scalar deltaT,
372  scalar deltaT0
373 )
374 {
375  // First leapfrog velocity adjust and motion part, required before
376  // force calculation
377 
378  if (Pstream::master())
379  {
380  vector aClip = a();
381  scalar aMag = mag(aClip);
382 
383  if (aMag > SMALL)
384  {
385  aClip /= aMag;
386  }
387 
388  if (aMag > aLim_)
389  {
390  WarningIn
391  (
392  "void Foam::sixDoFRigidBodyMotion::updatePosition"
393  "("
394  "scalar deltaT, "
395  "scalar deltaT0"
396  ")"
397  )
398  << "Limited acceleration " << a()
399  << " to " << aClip*aLim_
400  << endl;
401  }
402 
403  v() += 0.5*(1 - cDamp_)*deltaT0*aClip*min(aMag, aLim_);
404 
405  pi() += 0.5*(1 - cDamp_)*deltaT0*tau();
406 
407  // Leapfrog move part
408  centreOfMass() += deltaT*v();
409 
410  // Leapfrog orientation adjustment
411  rotate(Q(), pi(), deltaT);
412  }
413 
414  Pstream::scatter(motionState_);
415 }
416 
417 
419 (
420  const vector& fGlobal,
421  const vector& tauGlobal,
422  scalar deltaT
423 )
424 {
425  // Second leapfrog velocity adjust part, required after motion and
426  // force calculation
427 
428  if (Pstream::master())
429  {
430  a() = fGlobal/mass_;
431 
432  tau() = (Q().T() & tauGlobal);
433 
434  applyRestraints();
435 
436  applyConstraints(deltaT);
437 
438  vector aClip = a();
439  scalar aMag = mag(aClip);
440 
441  if (aMag > SMALL)
442  {
443  aClip /= aMag;
444  }
445 
446  if (aMag > aLim_)
447  {
448  WarningIn
449  (
450  "void Foam::sixDoFRigidBodyMotion::updateForce"
451  "("
452  "const vector& fGlobal, "
453  "const vector& tauGlobal, "
454  "scalar deltaT"
455  ")"
456  )
457  << "Limited acceleration " << a()
458  << " to " << aClip*aLim_
459  << endl;
460  }
461 
462  v() += 0.5*(1 - cDamp_)*deltaT*aClip*min(aMag, aLim_);
463 
464  pi() += 0.5*(1 - cDamp_)*deltaT*tau();
465 
466  if(report_)
467  {
468  status();
469  }
470  }
471 
472  Pstream::scatter(motionState_);
473 }
474 
475 
477 (
478  const pointField& positions,
479  const vectorField& forces,
480  scalar deltaT
481 )
482 {
483  vector fGlobal = vector::zero;
484 
485  vector tauGlobal = vector::zero;
486 
487  if (Pstream::master())
488  {
489  fGlobal = sum(forces);
490 
491  forAll(positions, i)
492  {
493  tauGlobal += (positions[i] - centreOfMass()) ^ forces[i];
494  }
495  }
496 
497  updateForce(fGlobal, tauGlobal, deltaT);
498 }
499 
500 
502 (
503  const point& pInitial,
504  const vector& deltaForce,
505  const vector& deltaMoment,
506  scalar deltaT
507 ) const
508 {
509  vector vTemp = v() + deltaT*(a() + deltaForce/mass_);
510 
511  vector piTemp = pi() + deltaT*(tau() + (Q().T() & deltaMoment));
512 
513  point centreOfMassTemp = centreOfMass() + deltaT*vTemp;
514 
515  tensor QTemp = Q();
516 
517  rotate(QTemp, piTemp, deltaT);
518 
519  return
520  (
521  centreOfMassTemp
522  + (QTemp & initialQ_.T() & (pInitial - initialCentreOfMass_))
523  );
524 }
525 
526 
528 (
529  const vector& vInitial,
530  const vector& deltaMoment,
531  scalar deltaT
532 ) const
533 {
534  vector piTemp = pi() + deltaT*(tau() + (Q().T() & deltaMoment));
535 
536  tensor QTemp = Q();
537 
538  rotate(QTemp, piTemp, deltaT);
539 
540  return (QTemp & initialQ_.T() & vInitial);
541 }
542 
543 
545 {
546  Info<< "Centre of mass: " << centreOfMass() << nl
547  << "Linear velocity: " << v() << nl
548  << "Angular velocity: " << omega()
549  << endl;
550 }
551 
552 
553 // ************************ vim: set sw=4 sts=4 et: ************************ //