src/postProcessing/functionObjects/forces/pointPatchFields/derived/sixDoFRigidBodyMotion/sixDoFRigidBodyMotion.C

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