src/foam/algorithms/rotation/RodriguesRotation.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 \*---------------------------------------------------------------------------*/
  25
  26 #include "RodriguesRotation.H"
  27 #include "mathematicalConstants.H"
  28
  29 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
  30
  31 Foam::tensor Foam::RodriguesRotation
  32 (
  33     const vector& rotationAxis,
  34     const scalar& rotationAngle,
  35     const bool inDegrees
  36 )
  37 {
  38     tensor rotTensor;
  39     scalar theta = rotationAngle;
  40
  41     if (inDegrees)
  42     {
  43         theta *= mathematicalConstant::pi/180.0;
  44     }
  45
  46     scalar sinTheta = sin(theta);
  47     scalar cosTheta = cos(theta);
  48     scalar oneMinusCosTheta = 1.0 - cosTheta;
  49
  50     scalar magRotAxis = mag(rotationAxis);
  51
  52     if (magRotAxis < SMALL)
  53     {
  54         FatalErrorIn
  55         (
  56             "tensor RodriguesRotation\n"
  57             "(\n"
  58             "    const vector& rotationAxis,\n"
  59             "    const scalar& rotationAngle\n"
  60             ")"
  61         )   << "Incorrectly defined axis: " << rotationAxis
  62             << abort(FatalError);
  63     }
  64
  65     vector unitVector = rotationAxis/magRotAxis;
  66
  67     scalar wx = unitVector.x();
  68     scalar wy = unitVector.y();
  69     scalar wz = unitVector.z();
  70
  71     rotTensor.xx() = cosTheta + sqr(wx)*oneMinusCosTheta;
  72     rotTensor.yy() = cosTheta + sqr(wy)*oneMinusCosTheta;
  73     rotTensor.zz() = cosTheta + sqr(wz)*oneMinusCosTheta;
  74
  75     rotTensor.xy() = wx*wy*oneMinusCosTheta - wz*sinTheta;
  76     rotTensor.xz() = wy*sinTheta + wx*wz*oneMinusCosTheta;
  77
  78     rotTensor.yx() =  wz*sinTheta + wx*wy*oneMinusCosTheta;
  79     rotTensor.yz() = -wx*sinTheta + wy*wz*oneMinusCosTheta;
  80
  81     rotTensor.zx() = -wy*sinTheta + wx*wz*oneMinusCosTheta;
  82     rotTensor.zy() =  wx*sinTheta + wy*wz*oneMinusCosTheta;
  83
  84     return rotTensor;
  85 }
  86
  87 Foam::tensor Foam::RodriguesRotation2
  88 (
  89     const vector& rotationAxis,
  90     const scalar& rotationAngle,
  91     const bool inDegrees
  92 )
  93 {
  94     scalar magRotAxis = mag(rotationAxis);
  95
  96     if (magRotAxis < SMALL)
  97     {
  98         FatalErrorIn
  99         (
 100             "tensor RodriguesRotation2\n"
 101             "(\n"
 102             "    const vector& rotationAxis,\n"
 103             "    const scalar& rotationAngle\n"
 104             ")"
 105         )   << "Incorrectly defined axis: " << rotationAxis
 106             << abort(FatalError);
 107     }
 108
 109     scalar theta = rotationAngle;
 110
 111     if (inDegrees)
 112     {
 113         theta *= mathematicalConstant::pi/180.0;
 114     }
 115
 116     const vector k = rotationAxis/magRotAxis;
 117     const scalar k1 = k[0];
 118     const scalar k2 = k[1];
 119     const scalar k3 = k[2];
 120
 121     // [k]_x
 122     const tensor rotationT (
 123           0, -k3,  k2,
 124          k3,   0, -k1,
 125         -k2,  k1,   0
 126     );
 127
 128     // kk' - I
 129     const tensor projectionT (
 130         k1*k1 - 1.0, k1*k2,       k1*k3,
 131         k2*k1,       k2*k2 - 1.0, k2*k3,
 132         k3*k1,       k3*k2,       k3*k3 - 1.0);
 133
 134     const scalar cosTheta = cos(theta);
 135     const scalar sinTheta = sin(theta);
 136
 137     return I + sinTheta*rotationT + (1 - cosTheta)*projectionT;
 138
 139 }
 140
 141 Foam::tensor Foam::RodriguesRotation
 142 (
 143     const vector& rotationAxis,
 144     const vector& vi,
 145     const vector& vf
 146 )
 147 {
 148     scalar magRotAxis = mag(rotationAxis);
 149
 150     if (magRotAxis < SMALL)
 151     {
 152         FatalErrorIn
 153         (
 154             "tensor RodriguesRotation\n"
 155             "(\n"
 156             "    const vector& rotationAxis,\n"
 157             "    const vector& vi\n"
 158             "    const vector& vf\n"
 159             ")"
 160         )   << "Incorrectly defined axis: " << rotationAxis
 161             << abort(FatalError);
 162     }
 163
 164     const vector k = rotationAxis/magRotAxis;
 165     const scalar k1 = k[0];
 166     const scalar k2 = k[1];
 167     const scalar k3 = k[2];
 168
 169     // [k]_x
 170     const tensor rotationT (
 171           0, -k3,  k2,
 172          k3,   0, -k1,
 173         -k2,  k1,   0
 174     );
 175
 176     // kk' - I
 177     const tensor projectionT (
 178         k1*k1 - 1.0, k1*k2,       k1*k3,
 179         k2*k1,       k2*k2 - 1.0, k2*k3,
 180         k3*k1,       k3*k2,       k3*k3 - 1.0);
 181
 182     // Project both vectors onto the plane defined by the rotation axis
 183     vector nvi = -(projectionT & vi);
 184     vector nvf = -(projectionT & vf);
 185     nvi = nvi/mag(nvi);
 186     nvf = nvf/mag(nvf);
 187
 188     const vector crossNviNvf = nvi ^ nvf;
 189     const scalar cosTheta = nvi & nvf;
 190     const scalar sinTheta = mag(crossNviNvf) * sign(crossNviNvf & k);
 191
 192     return I + sinTheta*rotationT + (1 - cosTheta)*projectionT;
 193 }
 194
 195 Foam::tensorField Foam::RodriguesRotation
 196 (
 197     const vector& rotationAxis,
 198     const vectorField& vi,
 199     const vectorField& vf
 200 )
 201 {
 202     scalar magRotAxis = mag(rotationAxis);
 203
 204     if (magRotAxis < SMALL)
 205     {
 206         FatalErrorIn
 207         (
 208             "tensor RodriguesRotation\n"
 209             "(\n"
 210             "    const vector& rotationAxis,\n"
 211             "    const vectorField& vi\n"
 212             "    const vectorField& vf\n"
 213             ")"
 214         )   << "Incorrectly defined axis: " << rotationAxis
 215             << abort(FatalError);
 216     }
 217
 218     const vector k = rotationAxis/magRotAxis;
 219     const scalar k1 = k[0];
 220     const scalar k2 = k[1];
 221     const scalar k3 = k[2];
 222
 223     // [k]_x
 224     const tensor rotationT (
 225           0, -k3,  k2,
 226          k3,   0, -k1,
 227         -k2,  k1,   0
 228     );
 229
 230     // kk' - I
 231     const tensor projectionT (
 232         k1*k1 - 1.0, k1*k2,       k1*k3,
 233         k2*k1,       k2*k2 - 1.0, k2*k3,
 234         k3*k1,       k3*k2,       k3*k3 - 1.0);
 235
 236     // Project both vectors onto the plane defined by the rotation axis
 237     vectorField nvi = -(projectionT & vi);
 238     vectorField nvf = -(projectionT & vf);
 239     nvi = nvi/mag(nvi);
 240     nvf = nvf/mag(nvf);
 241
 242     const vectorField crossNviNvf = nvi ^ nvf;
 243     const scalarField cosTheta = nvi & nvf;
 244     const scalarField sinTheta = mag(crossNviNvf) * sign(crossNviNvf & k);
 245
 246     const tensorField I_F(vi.size(), I);
 247     const tensorField rotationT_F(vi.size(), rotationT);
 248     const tensorField projectionT_F(vi.size(), projectionT);
 249
 250     return I_F + sinTheta*rotationT_F + (1 - cosTheta)*projectionT_F;
 251 }
 252
 253
 254 // ************************************************************************* //