src/ODE/sixDOF/finiteRotation/finiteRotation.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     finiteRotation
  27
  28 Author
  29     Dubravko Matijasevic, FSB Zagreb.  All rights reserved.
  30     Update by Hrvoje Jasak
  31
  32 \*---------------------------------------------------------------------------*/
  33
  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     return ur/(mag(ur) + SMALL);
  45 }
  46
  47
  48 Foam::scalar Foam::finiteRotation::rotAngle(const tensor& rotT)
  49 {
  50     // Alternative formulation: Daniel Schmode, 15/Feb/2009
  51     scalar x = rotT.zy() - rotT.yz();
  52     scalar y = rotT.xz() - rotT.zx();
  53     scalar z = rotT.yx() - rotT.xy();
  54
  55     scalar r = hypot(x, hypot(y, z));
  56     scalar t = tr(rotT);
  57
  58     return atan2(r, t - 1);
  59 }
  60
  61
  62 // * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
  63
  64 Foam::finiteRotation::finiteRotation(const HamiltonRodriguezRot& rot)
  65 :
  66     eInitial_(rot),
  67     rotTensor_(rot.R()),
  68     rotIncrementTensor_(tensor::zero)
  69 {}
  70
  71
  72 Foam::finiteRotation::finiteRotation
  73 (
  74     const vector& r,
  75     const scalar& angle
  76 )
  77 :
  78     eInitial_(HamiltonRodriguezRot(r, angle)),
  79     rotTensor_(eInitial_.R()),
  80     rotIncrementTensor_(tensor::zero)
  81 {}
  82
  83
  84 // * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
  85
  86 Foam::finiteRotation::~finiteRotation()
  87 {}
  88
  89
  90 // * * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * //
  91
  92 void Foam::finiteRotation::updateRotation(const HamiltonRodriguezRot& rot)
  93 {
  94     tensor rotR = rot.R();
  95
  96     rotIncrementTensor_ = (rotR & (rotTensor_.T()));
  97     rotTensor_ = rotR;
  98 }
  99
 100
 101 const Foam::HamiltonRodriguezRot& Foam::finiteRotation::eInitial() const
 102 {
 103     return eInitial_;
 104 }
 105
 106
 107 Foam::HamiltonRodriguezRot Foam::finiteRotation::eCurrent() const
 108 {
 109     return HamiltonRodriguezRot(rotVector(), rotAngle());
 110 }
 111
 112
 113 const Foam::tensor& Foam::finiteRotation::rotTensor() const
 114 {
 115     return rotTensor_;
 116 }
 117
 118
 119 Foam::vector Foam::finiteRotation::rotVector() const
 120 {
 121     return rotVector(rotTensor_);
 122 }
 123
 124
 125 Foam::scalar Foam::finiteRotation::rotAngle() const
 126 {
 127     return rotAngle(rotTensor_);
 128 }
 129
 130
 131 const Foam::tensor& Foam::finiteRotation::rotIncrementTensor() const
 132 {
 133     return rotIncrementTensor_;
 134 }
 135
 136
 137 Foam::vector Foam::finiteRotation::rotIncrementVector() const
 138 {
 139     return rotVector(rotIncrementTensor_);
 140 }
 141
 142
 143 Foam::scalar Foam::finiteRotation::rotIncrementAngle() const
 144 {
 145     return rotAngle(rotIncrementTensor_);
 146 }
 147
 148
 149 Foam::vector Foam::finiteRotation::omegaAverage(const scalar deltaT) const
 150 {
 151     return (rotIncrementAngle()/deltaT)*rotIncrementVector();
 152 }
 153
 154
 155 Foam::tensor Foam::finiteRotation::rotTensorAverage() const
 156 {
 157     return
 158     (
 159         HamiltonRodriguezRot
 160         (
 161             rotIncrementVector(),
 162             0.5*rotIncrementAngle()
 163         ).R().T()
 164       & rotTensor_
 165     );
 166 }
 167
 168
 169 Foam::vector Foam::finiteRotation::rotVectorAverage() const
 170 {
 171     return rotVector(rotTensorAverage());
 172 }
 173
 174
 175 Foam::vector Foam::finiteRotation::omegaAverageAbsolute
 176 (
 177     const scalar deltaT
 178 ) const
 179 {
 180     return (rotTensorAverage().T() & omegaAverage(deltaT));
 181 }
 182
 183
 184 // ************************************************************************* //