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     // Bugfix: sqr. SS, 18/Apr/2016
  88     const scalar c2 = sqrt(sqr(rotT.xx()) + sqr(rotT.xy()));
  89
  90     // Calculate pitch angle
  91     pitchAngle = atan2(-rotT.xz(), c2);
  92
  93     const scalar s1 = sin(rollAngle);
  94     const scalar c1 = cos(rollAngle);
  95
  96     // Calculate yaw angle
  97     yawAngle = atan2(s1*rotT.zx() - c1*rotT.yx(), c1*rotT.yy() - s1*rotT.zy());
  98
  99     return eulerAngles;
 100 }
 101
 102
 103 // * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
 104
 105 Foam::finiteRotation::finiteRotation(const HamiltonRodriguezRot& rot)
 106 :
 107     eInitial_(rot),
 108     rotTensor_(rot.R()),
 109     rotIncrementTensor_(tensor::zero)
 110 {}
 111
 112
 113 Foam::finiteRotation::finiteRotation
 114 (
 115     const vector& r,
 116     const scalar& angle
 117 )
 118 :
 119     eInitial_(HamiltonRodriguezRot(r, angle)),
 120     rotTensor_(eInitial_.R()),
 121     rotIncrementTensor_(tensor::zero)
 122 {}
 123
 124
 125 // * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
 126
 127 Foam::finiteRotation::~finiteRotation()
 128 {}
 129
 130
 131 // * * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * //
 132
 133 void Foam::finiteRotation::updateRotation(const HamiltonRodriguezRot& rot)
 134 {
 135     tensor rotR = rot.R();
 136
 137     rotIncrementTensor_ = (rotR & (rotTensor_.T()));
 138     rotTensor_ = rotR;
 139 }
 140
 141
 142 const Foam::HamiltonRodriguezRot& Foam::finiteRotation::eInitial() const
 143 {
 144     return eInitial_;
 145 }
 146
 147
 148 Foam::HamiltonRodriguezRot Foam::finiteRotation::eCurrent() const
 149 {
 150     return HamiltonRodriguezRot(rotVector(), rotAngle());
 151 }
 152
 153
 154 const Foam::tensor& Foam::finiteRotation::rotTensor() const
 155 {
 156     return rotTensor_;
 157 }
 158
 159
 160 Foam::vector Foam::finiteRotation::rotVector() const
 161 {
 162     return rotVector(rotTensor_);
 163 }
 164
 165
 166 Foam::scalar Foam::finiteRotation::rotAngle() const
 167 {
 168     return rotAngle(rotTensor_);
 169 }
 170
 171
 172 Foam::vector Foam::finiteRotation::eulerAngles() const
 173 {
 174     return eulerAngles(rotTensor_);
 175 }
 176
 177
 178 const Foam::tensor& Foam::finiteRotation::rotIncrementTensor() const
 179 {
 180     return rotIncrementTensor_;
 181 }
 182
 183
 184 Foam::vector Foam::finiteRotation::rotIncrementVector() const
 185 {
 186     return rotVector(rotIncrementTensor_);
 187 }
 188
 189
 190 Foam::scalar Foam::finiteRotation::rotIncrementAngle() const
 191 {
 192     return rotAngle(rotIncrementTensor_);
 193 }
 194
 195
 196 Foam::vector Foam::finiteRotation::omegaAverage(const scalar deltaT) const
 197 {
 198     return (rotIncrementAngle()/deltaT)*rotIncrementVector();
 199 }
 200
 201
 202 Foam::tensor Foam::finiteRotation::rotTensorAverage() const
 203 {
 204     return
 205     (
 206         HamiltonRodriguezRot
 207         (
 208             rotIncrementVector(),
 209             0.5*rotIncrementAngle()
 210         ).R().T()
 211       & rotTensor_
 212     );
 213 }
 214
 215
 216 Foam::vector Foam::finiteRotation::rotVectorAverage() const
 217 {
 218     return rotVector(rotTensorAverage());
 219 }
 220
 221
 222 Foam::vector Foam::finiteRotation::omegaAverageAbsolute
 223 (
 224     const scalar deltaT
 225 ) const
 226 {
 227     return (rotTensorAverage().T() & omegaAverage(deltaT));
 228 }
 229
 230
 231 // ************************************************************************* //