src/ODE/sixDOF/sixDOFqODE/sixDOFqODE.C

   1 /*---------------------------------------------------------------------------*\
   2   =========                 |
   3   \\      /  F ield         | foam-extend: Open Source CFD
   4    \\    /   O peration     | Version:     3.2
   5     \\  /    A nd           | Web:         http://www.foam-extend.org
   6      \\/     M anipulation  | For copyright notice see file Copyright
   7 -------------------------------------------------------------------------------
   8 License
   9     This file is part of foam-extend.
  10
  11     foam-extend 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 3 of the License, or (at your
  14     option) any later version.
  15
  16     foam-extend is distributed in the hope that it will be useful, but
  17     WITHOUT ANY WARRANTY; without even the implied warranty of
  18     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  19     General Public License for more details.
  20
  21     You should have received a copy of the GNU General Public License
  22     along with foam-extend.  If not, see <http://www.gnu.org/licenses/>.
  23
  24 Class
  25     sixDOFqODE
  26
  27 Description
  28     6-DOF solver using quaternions
  29
  30 Author
  31     Dubravko Matijasevic, FSB Zagreb.  All rights reserved.
  32
  33 \*---------------------------------------------------------------------------*/
  34
  35 #include "objectRegistry.H"
  36 #include "sixDOFqODE.H"
  37
  38 // * * * * * * * * * * * * * Private Member Functions  * * * * * * * * * * * //
  39
  40 void Foam::sixDOFqODE::setCoeffs()
  41 {
  42     // Set ODE coefficients from position and rotation
  43
  44     // Linear displacement relative to spring equilibrium
  45     const vector& Xval = Xrel_.value();
  46     coeffs_[0] = Xval.x();
  47     coeffs_[1] = Xval.y();
  48     coeffs_[2] = Xval.z();
  49
  50     // Linear velocity
  51     const vector& Uval = U_.value();
  52     coeffs_[3] = Uval.x();
  53     coeffs_[4] = Uval.y();
  54     coeffs_[5] = Uval.z();
  55
  56     // Rotational velocity in non - inertial coordinate system
  57     const vector& omegaVal = omega_.value();
  58     coeffs_[6] = omegaVal.x();
  59     coeffs_[7] = omegaVal.y();
  60     coeffs_[8] = omegaVal.z();
  61
  62     // Quaternions
  63     coeffs_[9] = rotation_.eInitial().e0();
  64     coeffs_[10] = rotation_.eInitial().e1();
  65     coeffs_[11] = rotation_.eInitial().e2();
  66     coeffs_[12] = rotation_.eInitial().e3();
  67 }
  68
  69
  70 Foam::dimensionedVector Foam::sixDOFqODE::A
  71 (
  72     const dimensionedVector& xR,
  73     const dimensionedVector& uR,
  74     const HamiltonRodriguezRot& rotation
  75 ) const
  76 {
  77     // Fix the global force for global rotation constraints
  78     dimensionedVector fAbs = force();
  79
  80     // Constrain translation
  81     constrainTranslation(fAbs.value());
  82
  83     return
  84     (
  85        - (linSpringCoeffs_ & xR)    // spring
  86        - (linDampingCoeffs_ & uR)   // damping
  87        + fAbs
  88          // To absolute
  89        + (rotation.invR() & forceRelative())
  90     )/mass_;
  91 }
  92
  93
  94 Foam::dimensionedVector Foam::sixDOFqODE::OmegaDot
  95 (
  96     const HamiltonRodriguezRot& rotation,
  97     const dimensionedVector& omega
  98 ) const
  99 {
 100     // Fix the global moment for global rotation constraints
 101     dimensionedVector mAbs = moment();
 102
 103     // Constrain rotation
 104     constrainRotation(mAbs.value());
 105
 106     return
 107         inv(momentOfInertia_)
 108       & (
 109             E(omega)
 110             // To relative
 111           + (rotation.R() & mAbs)
 112           + momentRelative()
 113         );
 114 }
 115
 116
 117 Foam::dimensionedVector Foam::sixDOFqODE::E
 118 (
 119     const dimensionedVector& omega
 120 ) const
 121 {
 122     return (*(momentOfInertia_ & omega) & omega);
 123 }
 124
 125
 126 void Foam::sixDOFqODE::constrainRotation(vector& vec) const
 127 {
 128     vector consVec(vector::zero);
 129
 130     // Constrain the vector in respect to referent or global coordinate system
 131     if (referentMotionConstraints_)
 132     {
 133         consVec = referentRotation_.R() & vec;
 134
 135         if (fixedRoll_)
 136         {
 137             consVec.x() = 0;
 138         }
 139         if (fixedPitch_)
 140         {
 141             consVec.y() = 0;
 142         }
 143         if (fixedYaw_)
 144         {
 145             consVec.z() = 0;
 146         }
 147
 148         vec = referentRotation_.invR() & consVec;
 149     }
 150     else
 151     {
 152         if (fixedRoll_)
 153         {
 154             vec.x() = 0;
 155         }
 156         if (fixedPitch_)
 157         {
 158             vec.y() = 0;
 159         }
 160         if (fixedYaw_)
 161         {
 162             vec.z() = 0;
 163         }
 164     }
 165 }
 166
 167
 168 void Foam::sixDOFqODE::constrainTranslation(vector& vec) const
 169 {
 170     vector consVec(vector::zero);
 171
 172     // Constrain the vector in respect to referent or global coordinate system
 173     if (referentMotionConstraints_)
 174     {
 175         consVec = referentRotation_.R() & vec;
 176
 177         if (fixedSurge_)
 178         {
 179             consVec.x() = 0;
 180         }
 181         if (fixedSway_)
 182         {
 183             consVec.y() = 0;
 184         }
 185         if (fixedHeave_)
 186         {
 187             consVec.z() = 0;
 188         }
 189
 190         vec = referentRotation_.invR() & consVec;
 191     }
 192     else
 193     {
 194         if (fixedSurge_)
 195         {
 196             vec.x() = 0;
 197         }
 198         if (fixedSway_)
 199         {
 200             vec.y() = 0;
 201         }
 202         if (fixedHeave_)
 203         {
 204             vec.z() = 0;
 205         }
 206     }
 207 }
 208
 209
 210 // * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
 211
 212 // Construct from components
 213 Foam::sixDOFqODE::sixDOFqODE(const IOobject& io)
 214 :
 215     IOdictionary(io),
 216
 217     mass_(lookup("mass")),
 218     momentOfInertia_(lookup("momentOfInertia")),
 219
 220     Xequilibrium_(lookup("equilibriumPosition")),
 221     linSpringCoeffs_(lookup("linearSpring")),
 222     linDampingCoeffs_(lookup("linearDamping")),
 223
 224     Xrel_(lookup("Xrel")),
 225     U_(lookup("U")),
 226     Uaverage_(U_),
 227     rotation_
 228     (
 229         vector(lookup("rotationVector")),
 230         dimensionedScalar(lookup("rotationAngle")).value()
 231     ),
 232     omega_(lookup("omega")),
 233     omegaAverage_(omega_),
 234     omegaAverageAbsolute_(omega_),
 235
 236     force_(lookup("force")),
 237     moment_(lookup("moment")),
 238     forceRelative_(lookup("forceRelative")),
 239     momentRelative_(lookup("momentRelative")),
 240
 241     coeffs_(13, 0.0),
 242
 243     fixedSurge_(lookup("fixedSurge")),
 244     fixedSway_(lookup("fixedSway")),
 245     fixedHeave_(lookup("fixedHeave")),
 246     fixedRoll_(lookup("fixedRoll")),
 247     fixedPitch_(lookup("fixedPitch")),
 248     fixedYaw_(lookup("fixedYaw")),
 249     referentMotionConstraints_
 250     (
 251         lookupOrDefault<Switch>
 252         (
 253             "referentMotionConstraints",
 254             false
 255         )
 256     ),
 257     referentRotation_(vector(1, 0, 0), 0)
 258 {
 259     setCoeffs();
 260 }
 261
 262
 263 // Construct as copy
 264 Foam::sixDOFqODE::sixDOFqODE
 265 (
 266     const word& name,
 267     const sixDOFqODE& sd
 268 )
 269 :
 270     IOdictionary
 271     (
 272         IOobject
 273         (
 274             name,
 275             sd.instance(),
 276             sd.local(),
 277             sd.db(),
 278             IOobject::NO_READ,
 279             IOobject::NO_WRITE
 280         )
 281     ),
 282
 283     mass_(sd.mass_.name(), sd.mass_),
 284     momentOfInertia_(sd.momentOfInertia_.name(), sd.momentOfInertia_),
 285
 286     Xequilibrium_(sd.Xequilibrium_.name(), sd.Xequilibrium_),
 287     linSpringCoeffs_(sd.linSpringCoeffs_.name(), sd.linSpringCoeffs_),
 288     linDampingCoeffs_(sd.linDampingCoeffs_.name(), sd.linDampingCoeffs_),
 289
 290     Xrel_(sd.Xrel_.name(), sd.Xrel_),
 291     U_(sd.U_.name(), sd.U_),
 292     Uaverage_(sd.Uaverage_.name(), sd.Uaverage_),
 293     rotation_(sd.rotation_),
 294     omega_(sd.omega_.name(), sd.omega_),
 295     omegaAverage_(sd.omegaAverage_.name(), sd.omegaAverage_),
 296     omegaAverageAbsolute_
 297     (
 298         sd.omegaAverageAbsolute_.name(),
 299         sd.omegaAverageAbsolute_
 300     ),
 301
 302     force_(sd.force_.name(), sd.force_),
 303     moment_(sd.moment_.name(), sd.moment_),
 304     forceRelative_(sd.forceRelative_.name(), sd.forceRelative_),
 305     momentRelative_(sd.momentRelative_.name(), sd.momentRelative_),
 306
 307     coeffs_(sd.coeffs_),
 308
 309     fixedSurge_(sd.fixedSurge_),
 310     fixedSway_(sd.fixedSway_),
 311     fixedHeave_(sd.fixedHeave_),
 312     fixedRoll_(sd.fixedRoll_),
 313     fixedPitch_(sd.fixedPitch_),
 314     fixedYaw_(sd.fixedYaw_),
 315     referentMotionConstraints_(sd.referentMotionConstraints_),
 316     referentRotation_(sd.referentRotation_)
 317 {}
 318
 319
 320 // * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
 321
 322 Foam::sixDOFqODE::~sixDOFqODE()
 323 {}
 324
 325
 326 // * * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * //
 327
 328 void Foam::sixDOFqODE::setState(const sixDOFqODE& sd)
 329 {
 330     mass_ = sd.mass_;
 331     momentOfInertia_ = sd.momentOfInertia_;
 332
 333     Xequilibrium_ = sd.Xequilibrium_;
 334     linSpringCoeffs_ = sd.linSpringCoeffs_;
 335     linDampingCoeffs_ = sd.linDampingCoeffs_;
 336
 337     Xrel_ = sd.Xrel_;
 338     U_ = sd.U_;
 339     Uaverage_ = sd.Uaverage_;
 340     rotation_ = sd.rotation_;
 341     omega_ = sd.omega_;
 342     omegaAverage_ = sd.omegaAverage_;
 343     omegaAverageAbsolute_ = sd.omegaAverageAbsolute_;
 344
 345     force_ = sd.force_;
 346     moment_ = sd.moment_;
 347     forceRelative_ = sd.forceRelative_;
 348     momentRelative_ = sd.momentRelative_;
 349
 350     // Copy ODE coefficients: this carries actual ODE state
 351     // HJ, 23/Mar/2015
 352     coeffs_ = sd.coeffs_;
 353
 354     fixedSurge_ = sd.fixedSurge_;
 355     fixedSway_ = sd.fixedSway_;
 356     fixedHeave_ = sd.fixedHeave_;
 357     fixedRoll_ = sd.fixedRoll_;
 358     fixedPitch_ = sd.fixedPitch_;
 359     fixedYaw_ = sd.fixedYaw_;
 360 }
 361
 362
 363 void Foam::sixDOFqODE::derivatives
 364 (
 365     const scalar x,
 366     const scalarField& y,
 367     scalarField& dydx
 368 ) const
 369 {
 370     // Set the derivatives for displacement
 371     dydx[0] = y[3];
 372     dydx[1] = y[4];
 373     dydx[2] = y[5];
 374
 375     dimensionedVector curX("curX", dimLength, vector(y[0], y[1], y[2]));
 376     dimensionedVector curU("curU", dimVelocity, vector(y[3], y[4], y[5]));
 377     const HamiltonRodriguezRot curRotation
 378     (
 379         y[9],
 380         y[10],
 381         y[11],
 382         y[12]
 383     );
 384
 385     const vector accel = A(curX, curU, curRotation).value();
 386
 387     dydx[3] = accel.x();
 388     dydx[4] = accel.y();
 389     dydx[5] = accel.z();
 390
 391 //    // Add translational constraints by setting RHS of given components to zero
 392 //    if (fixedSurge_)
 393 //    {
 394 //        dydx[0] = 0; // Surge velocity
 395 //        dydx[3] = 0; // Surge acceleration
 396 //    }
 397 //    if (fixedSway_)
 398 //    {
 399 //        dydx[1] = 0; // Sway velocity
 400 //        dydx[4] = 0; // Sway acceleration
 401 //    }
 402 //    if (fixedHeave_)
 403 //    {
 404 //        dydx[2] = 0; // Heave velocity
 405 //        dydx[5] = 0; // Heave acceleration
 406 //    }
 407
 408     // Set the derivatives for rotation
 409     dimensionedVector curOmega
 410     (
 411         "curOmega",
 412         dimless/dimTime,
 413         vector(y[6], y[7], y[8])
 414     );
 415
 416 //    dimensionedVector curGlobalOmega = curRotation.invR() & curOmega;
 417 //
 418 //    // Add rotational constraints by setting RHS of given components to zero
 419 //    if (fixedRoll_)
 420 //    {
 421 //        curGlobalOmega.value().x() = 0;
 422 //    }
 423 //    if (fixedPitch_)
 424 //    {
 425 //        curGlobalOmega.value().y() = 0;
 426 //    }
 427 //    if (fixedYaw_)
 428 //    {
 429 //        curGlobalOmega.value().z() = 0;
 430 //    }
 431 //
 432 //
 433 //    curOmega = curRotation.R() & curGlobalOmega;
 434
 435     const vector omegaDot = OmegaDot(curRotation, curOmega).value();
 436
 437     dydx[6] = omegaDot.x();
 438     dydx[7] = omegaDot.y();
 439     dydx[8] = omegaDot.z();
 440
 441     dydx[9] = curRotation.eDot(curOmega.value(), 0);
 442     dydx[10] = curRotation.eDot(curOmega.value(), 1);
 443     dydx[11] = curRotation.eDot(curOmega.value(), 2);
 444     dydx[12] = curRotation.eDot(curOmega.value(), 3);
 445
 446     // Add rotational constraints by setting RHS of given components to zero
 447     if (fixedRoll_)
 448     {
 449         dydx[10] = 0; // Roll axis (roll quaternion evolution RHS)
 450     }
 451     if (fixedPitch_)
 452     {
 453         dydx[11] = 0; // Pitch axis (pitch quaternion evolution RHS)
 454     }
 455     if (fixedYaw_)
 456     {
 457         dydx[12] = 0; // Yaw axis (yaw quaternion evolution RHS)
 458     }
 459 }
 460
 461
 462 void Foam::sixDOFqODE::jacobian
 463 (
 464     const scalar x,
 465     const scalarField& y,
 466     scalarField& dfdx,
 467     scalarSquareMatrix& dfdy
 468 ) const
 469 {
 470     Info << "jacobian(...)" << endl;
 471     notImplemented("sixDOFqODE::jacobian(...) const");
 472 }
 473
 474
 475 void Foam::sixDOFqODE::update(const scalar delta)
 476 {
 477     // Update position
 478     vector Xold = Xrel_.value();
 479
 480     vector& Xval = Xrel_.value();
 481
 482     Xval.x() = coeffs_[0];
 483     Xval.y() = coeffs_[1];
 484     Xval.z() = coeffs_[2];
 485
 486     // Update velocity
 487     Uaverage_.value() = (Xval - Xold)/delta;
 488
 489     vector& Uval = U_.value();
 490
 491     Uval.x() = coeffs_[3];
 492     Uval.y() = coeffs_[4];
 493     Uval.z() = coeffs_[5];
 494
 495     // Constrain velocity
 496     constrainTranslation(Uval);
 497     coeffs_[3] = Uval.x();
 498     coeffs_[4] = Uval.y();
 499     coeffs_[5] = Uval.z();
 500
 501     // Update omega
 502     vector& omegaVal = omega_.value();
 503
 504     omegaVal.x() = coeffs_[6];
 505     omegaVal.y() = coeffs_[7];
 506     omegaVal.z() = coeffs_[8];
 507
 508     // Constrain omega
 509     constrainRotation(omegaVal);
 510     coeffs_[6] = omegaVal.x();
 511     coeffs_[7] = omegaVal.y();
 512     coeffs_[8] = omegaVal.z();
 513
 514     rotation_.updateRotation
 515     (
 516         HamiltonRodriguezRot
 517         (
 518             coeffs_[9],
 519             coeffs_[10],
 520             coeffs_[11],
 521             coeffs_[12]
 522         )
 523     );
 524
 525     omegaAverage_.value() = rotation_.omegaAverage(delta);
 526
 527     // Calculate and constrain omegaAverageAbsolute appropriately
 528     vector& omegaAverageAbsoluteValue = omegaAverageAbsolute_.value();
 529     omegaAverageAbsoluteValue = rotation_.omegaAverageAbsolute(delta);
 530
 531     if (fixedRoll_)
 532     {
 533         omegaAverageAbsoluteValue.x() = 0;
 534     }
 535     if (fixedPitch_)
 536     {
 537         omegaAverageAbsoluteValue.y() = 0;
 538     }
 539     if (fixedYaw_)
 540     {
 541         omegaAverageAbsoluteValue.z() = 0;
 542     }
 543 }
 544
 545
 546 // * * * * * * * * * * * * * * * Friend Operators  * * * * * * * * * * * * * //
 547
 548 bool Foam::sixDOFqODE::writeData(Ostream& os) const
 549 {
 550     os << *this;
 551     return os.good();
 552 }
 553
 554
 555 // * * * * * * * * * * * * * * IOstream Operators  * * * * * * * * * * * * * //
 556
 557 Foam::Ostream& Foam::operator<<(Ostream& os, const sixDOFqODE& sds)
 558 {
 559     os.writeKeyword("mass") << tab << sds.mass_ << token::END_STATEMENT << nl;
 560     os.writeKeyword("momentOfInertia") << tab << sds.momentOfInertia_
 561         << token::END_STATEMENT << nl << nl;
 562
 563     os.writeKeyword("equilibriumPosition") << tab << sds.Xequilibrium_
 564         << token::END_STATEMENT << nl;
 565     os.writeKeyword("linearSpring") << tab << sds.linSpringCoeffs_
 566         << token::END_STATEMENT << nl;
 567     os.writeKeyword("linearDamping") << tab << sds.linDampingCoeffs_
 568         << token::END_STATEMENT << nl << nl;
 569
 570     os.writeKeyword("Xrel") << tab << sds.Xrel() << token::END_STATEMENT << nl;
 571     os.writeKeyword("U") << tab << sds.U() << token::END_STATEMENT << nl;
 572     os.writeKeyword("rotationVector") << tab << sds.rotVector()
 573         << token::END_STATEMENT << nl;
 574     os.writeKeyword("rotationAngle") << tab << sds.rotAngle()
 575         << token::END_STATEMENT << nl;
 576     os.writeKeyword("omega") << tab << sds.omega()
 577         << token::END_STATEMENT << nl << nl;
 578
 579     os.writeKeyword("force") << tab << sds.force()
 580         << token::END_STATEMENT << nl;
 581     os.writeKeyword("moment") << tab << sds.moment()
 582         << token::END_STATEMENT << nl;
 583     os.writeKeyword("forceRelative") << tab << sds.forceRelative()
 584         << token::END_STATEMENT << nl;
 585     os.writeKeyword("momentRelative") << tab << sds.momentRelative()
 586         << token::END_STATEMENT << endl;
 587
 588     os.writeKeyword("fixedSurge") << tab << sds.fixedSurge_ <<
 589         token::END_STATEMENT << endl;
 590     os.writeKeyword("fixedSway") << tab << sds.fixedSway_ <<
 591         token::END_STATEMENT << endl;
 592     os.writeKeyword("fixedHeave") << tab << sds.fixedHeave_ <<
 593         token::END_STATEMENT << endl;
 594     os.writeKeyword("fixedRoll") << tab << sds.fixedRoll_ <<
 595         token::END_STATEMENT << endl;
 596     os.writeKeyword("fixedPitch") << tab << sds.fixedPitch_ <<
 597         token::END_STATEMENT << endl;
 598     os.writeKeyword("fixedYaw") << tab << sds.fixedYaw_ <<
 599         token::END_STATEMENT << endl;
 600
 601     return os;
 602 }
 603
 604
 605 // ************************************************************************* //