src/ODE/sixDOF/sixDOFqODE/sixDOFqODE.C

   1 /*---------------------------------------------------------------------------*\
   2   =========                 |
   3   \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
   4    \\    /   O peration     |
   5     \\  /    A nd           | Copyright held by original author
   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 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, write to the Free Software Foundation,
  23     Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
  24
  25 Class
  26     sixDOFqODE
  27
  28 Description
  29     6-DOF solver using quaternions
  30
  31 Author
  32     Dubravko Matijasevic, FSB Zagreb.  All rights reserved.
  33
  34 \*---------------------------------------------------------------------------*/
  35
  36 #include "sixDOFqODE.H"
  37
  38 // * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
  39
  40 // Runtime type information
  41 // Not possible because of I/O error: incorrect type, expecting dictionary
  42 // HJ, 11/Feb/2008
  43 // namespace Foam
  44 // {
  45 //     defineTypeNameAndDebug(sixDOFqODE, 0);
  46 // }
  47
  48
  49 // * * * * * * * * * * * * * Private Member Functions  * * * * * * * * * * * //
  50
  51 void Foam::sixDOFqODE::setCoeffs()
  52 {
  53     // Set ODE coefficients from position and rotation
  54
  55     // Linear displacement relative to spring equilibrium
  56     const vector& Xval = Xrel_.value();
  57     coeffs_[0] = Xval.x();
  58     coeffs_[1] = Xval.y();
  59     coeffs_[2] = Xval.z();
  60
  61     // Linear velocity
  62     const vector& Uval = U_.value();
  63     coeffs_[3] = Uval.x();
  64     coeffs_[4] = Uval.y();
  65     coeffs_[5] = Uval.z();
  66
  67     // Rotational velocity in non - inertial coordinate system
  68     const vector& omegaVal = omega_.value();
  69     coeffs_[6] = omegaVal.x();
  70     coeffs_[7] = omegaVal.y();
  71     coeffs_[8] = omegaVal.z();
  72
  73     // Quaternions
  74     coeffs_[9] = rotation_.eInitial().e0();
  75     coeffs_[10] = rotation_.eInitial().e1();
  76     coeffs_[11] = rotation_.eInitial().e2();
  77     coeffs_[12] = rotation_.eInitial().e3();
  78 }
  79
  80
  81 Foam::dimensionedVector Foam::sixDOFqODE::A
  82 (
  83     const dimensionedVector& xR,
  84     const dimensionedVector& uR,
  85     const HamiltonRodriguezRot& rotation
  86 ) const
  87 {
  88     return
  89     (
  90        - (linSpringCoeffs_ & xR)    // spring
  91        - (linDampingCoeffs_ & uR)   // damping
  92        + force()
  93          // To absolute
  94        + (rotation.invR() & forceRelative())
  95     )/mass_;
  96 }
  97
  98
  99 Foam::dimensionedVector Foam::sixDOFqODE::OmegaDot
 100 (
 101     const HamiltonRodriguezRot& rotation,
 102     const dimensionedVector& omega
 103 ) const
 104 {
 105     return
 106         inv(momentOfInertia_)
 107       & (
 108             E(omega)
 109             // To relative
 110           + (rotation.R() & moment())
 111           + momentRelative()
 112         );
 113 }
 114
 115
 116 Foam::dimensionedVector Foam::sixDOFqODE::E
 117 (
 118     const dimensionedVector& omega
 119 ) const
 120 {
 121     return (*(momentOfInertia_ & omega) & omega);
 122 }
 123
 124
 125
 126 // * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
 127
 128 // Construct from components
 129 Foam::sixDOFqODE::sixDOFqODE(const IOobject& io)
 130 :
 131     IOdictionary(io),
 132     mass_(lookup("mass")),
 133     momentOfInertia_(lookup("momentOfInertia")),
 134     Xequilibrium_(lookup("equilibriumPosition")),
 135     linSpringCoeffs_(lookup("linearSpring")),
 136     linDampingCoeffs_(lookup("linearDamping")),
 137     Xrel_(lookup("Xrel")),
 138     U_(lookup("U")),
 139     Uaverage_(U_),
 140     rotation_
 141     (
 142         vector(lookup("rotationVector")),
 143         dimensionedScalar(lookup("rotationAngle")).value()
 144     ),
 145     omega_(lookup("omega")),
 146     omegaAverage_(omega_),
 147     omegaAverageAbsolute_(omega_),
 148     force_(lookup("force")),
 149     moment_(lookup("moment")),
 150     forceRelative_(lookup("forceRelative")),
 151     momentRelative_(lookup("momentRelative")),
 152     coeffs_(13, 0.0)
 153 {
 154     setCoeffs();
 155 }
 156
 157
 158 // * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
 159
 160 Foam::sixDOFqODE::~sixDOFqODE()
 161 {}
 162
 163
 164 // * * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * //
 165
 166 void Foam::sixDOFqODE::derivatives
 167 (
 168     const scalar x,
 169     const scalarField& y,
 170     scalarField& dydx
 171 ) const
 172 {
 173     // Set the derivatives for displacement
 174     dydx[0] = y[3];
 175     dydx[1] = y[4];
 176     dydx[2] = y[5];
 177
 178     dimensionedVector curX("curX", dimLength, vector(y[0], y[1], y[2]));
 179     dimensionedVector curU("curU", dimVelocity, vector(y[3], y[4], y[5]));
 180     const HamiltonRodriguezRot curRotation
 181     (
 182         y[9],
 183         y[10],
 184         y[11],
 185         y[12]
 186     );
 187
 188     const vector accel = A(curX, curU, curRotation).value();
 189
 190     dydx[3] = accel.x();
 191     dydx[4] = accel.y();
 192     dydx[5] = accel.z();
 193
 194     // Set the derivatives for rotation
 195     dimensionedVector curOmega
 196     (
 197         "curOmega",
 198         dimless/dimTime,
 199         vector(y[6], y[7], y[8])
 200     );
 201
 202     const vector omegaDot = OmegaDot(curRotation, curOmega).value();
 203
 204     dydx[6]  = omegaDot.x();
 205     dydx[7] = omegaDot.y();
 206     dydx[8] = omegaDot.z();
 207
 208     dydx[9] = curRotation.eDot(curOmega.value(), 0);
 209     dydx[10] = curRotation.eDot(curOmega.value(), 1);
 210     dydx[11] = curRotation.eDot(curOmega.value(), 2);
 211     dydx[12] = curRotation.eDot(curOmega.value(), 3);
 212 }
 213
 214
 215 void Foam::sixDOFqODE::jacobian
 216 (
 217     const scalar x,
 218     const scalarField& y,
 219     scalarField& dfdx,
 220     scalarSquareMatrix& dfdy
 221 ) const
 222 {
 223     Info << "jacobian(...)" << endl;
 224     notImplemented("sixDOFqODE::jacobian(...) const");
 225 }
 226
 227
 228 void Foam::sixDOFqODE::update(const scalar delta)
 229 {
 230     // Update position
 231     vector Xold = Xrel_.value();
 232
 233     vector& Xval = Xrel_.value();
 234
 235     Xval.x() = coeffs_[0];
 236     Xval.y() = coeffs_[1];
 237     Xval.z() = coeffs_[2];
 238
 239     // Update velocity
 240     Uaverage_.value() = (Xval - Xold)/delta;
 241
 242     vector& Uval = U_.value();
 243
 244     Uval.x() = coeffs_[3];
 245     Uval.y() = coeffs_[4];
 246     Uval.z() = coeffs_[5];
 247
 248     // Update omega
 249     vector& omegaVal = omega_.value();
 250
 251     omegaVal.x() = coeffs_[6];
 252     omegaVal.y() = coeffs_[7];
 253     omegaVal.z() = coeffs_[8];
 254
 255     rotation_.updateRotation
 256     (
 257         HamiltonRodriguezRot
 258         (
 259             coeffs_[9],
 260             coeffs_[10],
 261             coeffs_[11],
 262             coeffs_[12]
 263         )
 264     );
 265
 266     omegaAverage_.value() = rotation_.omegaAverage(delta);
 267     omegaAverageAbsolute_.value() = rotation_.omegaAverageAbsolute(delta);
 268 }
 269
 270
 271 // * * * * * * * * * * * * * * * Friend Operators  * * * * * * * * * * * * * //
 272
 273 bool Foam::sixDOFqODE::writeData(Ostream& os) const
 274 {
 275     os << *this;
 276     return os.good();
 277 }
 278
 279
 280 // * * * * * * * * * * * * * * IOstream Operators  * * * * * * * * * * * * * //
 281
 282 Foam::Ostream& Foam::operator<<(Ostream& os, const sixDOFqODE& sds)
 283 {
 284     os.writeKeyword("mass") << tab << sds.mass_ << token::END_STATEMENT << nl;
 285     os.writeKeyword("momentOfInertia") << tab << sds.momentOfInertia_
 286         << token::END_STATEMENT << nl << nl;
 287
 288     os.writeKeyword("equilibriumPosition") << tab << sds.Xequilibrium_
 289         << token::END_STATEMENT << nl;
 290     os.writeKeyword("linearSpring") << tab << sds.linSpringCoeffs_
 291         << token::END_STATEMENT << nl;
 292     os.writeKeyword("linearDamping") << tab << sds.linDampingCoeffs_
 293         << token::END_STATEMENT << nl << nl;
 294
 295     os.writeKeyword("Xrel") << tab << sds.Xrel() << token::END_STATEMENT << nl;
 296     os.writeKeyword("U") << tab << sds.U() << token::END_STATEMENT << nl;
 297     os.writeKeyword("rotationVector") << tab << sds.rotVector()
 298         << token::END_STATEMENT << nl;
 299     os.writeKeyword("rotationAngle") << tab << sds.rotAngle()
 300         << token::END_STATEMENT << nl;
 301     os.writeKeyword("omega") << tab << sds.omega()
 302         << token::END_STATEMENT << nl << nl;
 303
 304     os.writeKeyword("force") << tab << sds.force()
 305         << token::END_STATEMENT << nl;
 306     os.writeKeyword("moment") << tab << sds.moment()
 307         << token::END_STATEMENT << nl;
 308     os.writeKeyword("forceRelative") << tab << sds.forceRelative()
 309         << token::END_STATEMENT << nl;
 310     os.writeKeyword("momentRelative") << tab << sds.momentRelative()
 311         << token::END_STATEMENT << endl;
 312
 313     return os;
 314 }
 315
 316
 317 // ************************************************************************* //