src/ODE/sixDOF/finiteRotation/finiteRotation.C

   1 /*---------------------------------------------------------------------------*\
   2   =========                 |
   3   \\      /  F ield         | foam-extend: Open Source CFD
   4    \\    /   O peration     |
   5     \\  /    A nd           | For copyright notice see file Copyright
   6      \\/     M anipulation  |
   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     finiteRotation
  26
  27 Author
  28     Dubravko Matijasevic, FSB Zagreb.  All rights reserved.
  29     Update by Hrvoje Jasak
  30
  31 \*---------------------------------------------------------------------------*/
  32
  33 #include "objectRegistry.H"
  34 #include "finiteRotation.H"
  35
  36 // * * * * * * * * * * * * * Private Member Functions  * * * * * * * * * * * //
  37
  38 Foam::vector Foam::finiteRotation::rotVector(const tensor& rotT)
  39 {
  40     vector ur = - *( inv(I + rotT) & (I - rotT) );
  41
  42     // Scaling to a unit vector.  HJ, problems with round-off
  43     // HJ, 4/Aug/2008
  44
  45     if (mag(ur) > SMALL)
  46     {
  47         return ur/(mag(ur) + SMALL);
  48     }
  49     else
  50     {
  51         // Rotation vector is undertermined at zero rotation
  52         // Returning arbitrary unit vector
  53         // HJ, 4/Mar/2015
  54         return vector(0, 0, 1);
  55     }
  56 }
  57
  58
  59 Foam::scalar Foam::finiteRotation::rotAngle(const tensor& rotT)
  60 {
  61     // Alternative formulation: Daniel Schmode, 15/Feb/2009
  62     scalar x = rotT.zy() - rotT.yz();
  63     scalar y = rotT.xz() - rotT.zx();
  64     scalar z = rotT.yx() - rotT.xy();
  65
  66     scalar r = hypot(x, hypot(y, z));
  67     scalar t = tr(rotT);
  68
  69     return atan2(r, t - 1);
  70 }
  71
  72
  73 Foam::vector Foam::finiteRotation::eulerAngles(const tensor& rotT)
  74 {
  75     // Create a vector containing euler angles (x = roll, y = pitch, z = yaw)
  76     vector eulerAngles;
  77
  78     scalar& rollAngle = eulerAngles.x();
  79     scalar& pitchAngle = eulerAngles.y();
  80     scalar& yawAngle = eulerAngles.z();
  81
  82     // Calculate roll angle
  83     rollAngle = atan2(rotT.yz(), rotT.zz());
  84
  85     // Use mag to avoid negative value due to round-off
  86     // HJ, 24/Feb/2016
  87     const scalar c2 = sqrt(Foam::max(0, rotT.xx() + rotT.xy()));
  88
  89     // Calculate pitch angle
  90     pitchAngle = atan2(-rotT.xz(), c2);
  91
  92     const scalar s1 = sin(rollAngle);
  93     const scalar c1 = cos(rollAngle);
  94
  95     // Calculate yaw angle
  96     yawAngle = atan2(s1*rotT.zx() - c1*rotT.yx(), c1*rotT.yy() - s1*rotT.zy());
  97
  98     return eulerAngles;
  99 }
 100
 101
 102 // * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
 103
 104 Foam::finiteRotation::finiteRotation(const HamiltonRodriguezRot& rot)
 105 :
 106     eInitial_(rot),
 107     rotTensor_(rot.R()),
 108     rotIncrementTensor_(tensor::zero)
 109 {}
 110
 111
 112 Foam::finiteRotation::finiteRotation
 113 (
 114     const vector& r,
 115     const scalar& angle
 116 )
 117 :
 118     eInitial_(HamiltonRodriguezRot(r, angle)),
 119     rotTensor_(eInitial_.R()),
 120     rotIncrementTensor_(tensor::zero)
 121 {}
 122
 123
 124 // * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
 125
 126 Foam::finiteRotation::~finiteRotation()
 127 {}
 128
 129
 130 // * * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * //
 131
 132 void Foam::finiteRotation::updateRotation(const HamiltonRodriguezRot& rot)
 133 {
 134     tensor rotR = rot.R();
 135
 136     rotIncrementTensor_ = (rotR & (rotTensor_.T()));
 137     rotTensor_ = rotR;
 138 }
 139
 140
 141 const Foam::HamiltonRodriguezRot& Foam::finiteRotation::eInitial() const
 142 {
 143     return eInitial_;
 144 }
 145
 146
 147 Foam::HamiltonRodriguezRot Foam::finiteRotation::eCurrent() const
 148 {
 149     return HamiltonRodriguezRot(rotVector(), rotAngle());
 150 }
 151
 152
 153 const Foam::tensor& Foam::finiteRotation::rotTensor() const
 154 {
 155     return rotTensor_;
 156 }
 157
 158
 159 Foam::vector Foam::finiteRotation::rotVector() const
 160 {
 161     return rotVector(rotTensor_);
 162 }
 163
 164
 165 Foam::scalar Foam::finiteRotation::rotAngle() const
 166 {
 167     return rotAngle(rotTensor_);
 168 }
 169
 170
 171 Foam::vector Foam::finiteRotation::eulerAngles() const
 172 {
 173     return eulerAngles(rotTensor_);
 174 }
 175
 176
 177 const Foam::tensor& Foam::finiteRotation::rotIncrementTensor() const
 178 {
 179     return rotIncrementTensor_;
 180 }
 181
 182
 183 Foam::vector Foam::finiteRotation::rotIncrementVector() const
 184 {
 185     return rotVector(rotIncrementTensor_);
 186 }
 187
 188
 189 Foam::scalar Foam::finiteRotation::rotIncrementAngle() const
 190 {
 191     return rotAngle(rotIncrementTensor_);
 192 }
 193
 194
 195 Foam::vector Foam::finiteRotation::omegaAverage(const scalar deltaT) const
 196 {
 197     return (rotIncrementAngle()/deltaT)*rotIncrementVector();
 198 }
 199
 200
 201 Foam::tensor Foam::finiteRotation::rotTensorAverage() const
 202 {
 203     return
 204     (
 205         HamiltonRodriguezRot
 206         (
 207             rotIncrementVector(),
 208             0.5*rotIncrementAngle()
 209         ).R().T()
 210       & rotTensor_
 211     );
 212 }
 213
 214
 215 Foam::vector Foam::finiteRotation::rotVectorAverage() const
 216 {
 217     return rotVector(rotTensorAverage());
 218 }
 219
 220
 221 Foam::vector Foam::finiteRotation::omegaAverageAbsolute
 222 (
 223     const scalar deltaT
 224 ) const
 225 {
 226     return (rotTensorAverage().T() & omegaAverage(deltaT));
 227 }
 228
 229
 230 // ************************************************************************* //