src/OpenFOAM/primitives/quaternion/quaternionI.H

   1 /*---------------------------------------------------------------------------*\
   2   =========                 |
   3   \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
   4    \\    /   O peration     |
   5     \\  /    A nd           | Copyright (C) 1991-2009 OpenCFD Ltd.
   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 \*---------------------------------------------------------------------------*/
  26
  27 // * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
  28
  29 inline Foam::quaternion::quaternion()
  30 {}
  31
  32 inline Foam::quaternion::quaternion(const scalar w, const vector& v)
  33 :
  34     w_(w),
  35     v_(v)
  36 {}
  37
  38 inline Foam::quaternion::quaternion(const vector& d, const scalar theta)
  39 :
  40     w_(cos(0.5*theta)),
  41     v_((sin(0.5*theta)/magSqr(d))*d)
  42 {
  43     normalize();
  44 }
  45
  46 inline Foam::quaternion::quaternion(const scalar w)
  47 :
  48     w_(w),
  49     v_(vector::zero)
  50 {}
  51
  52 inline Foam::quaternion::quaternion(const vector& v)
  53 :
  54     w_(0),
  55     v_(v)
  56 {}
  57
  58 inline Foam::quaternion::quaternion
  59 (
  60     const scalar angleX,
  61     const scalar angleY,
  62     const scalar angleZ
  63 )
  64 {
  65     operator=(quaternion(vector(1, 0, 0), angleX));
  66     operator*=(quaternion(vector(0, 1, 0), angleY));
  67     operator*=(quaternion(vector(0, 0, 1), angleZ));
  68 }
  69
  70
  71 // * * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * //
  72
  73 inline Foam::scalar Foam::quaternion::w() const
  74 {
  75     return w_;
  76 }
  77
  78
  79 inline const Foam::vector& Foam::quaternion::v() const
  80 {
  81     return v_;
  82 }
  83
  84
  85 inline Foam::scalar& Foam::quaternion::w()
  86 {
  87     return w_;
  88 }
  89
  90
  91 inline Foam::vector& Foam::quaternion::v()
  92 {
  93     return v_;
  94 }
  95
  96
  97 inline void Foam::quaternion::normalize()
  98 {
  99     operator/=(mag(*this));
 100 }
 101
 102
 103 inline Foam::quaternion Foam::quaternion::mulq0v(const vector& u) const
 104 {
 105     return quaternion(-(v() & u), w()*u + (v() ^ u));
 106 }
 107
 108
 109 inline Foam::vector Foam::quaternion::transform(const vector& u) const
 110 {
 111     return (mulq0v(u)*conjugate(*this)).v();
 112 }
 113
 114
 115 inline Foam::vector Foam::quaternion::invTransform(const vector& u) const
 116 {
 117     return (conjugate(*this).mulq0v(u)*(*this)).v();
 118 }
 119
 120
 121 inline Foam::quaternion Foam::quaternion::transform(const quaternion& q) const
 122 {
 123     return Foam::normalize((*this)*q);
 124 }
 125
 126
 127 inline Foam::quaternion Foam::quaternion::invTransform
 128 (
 129     const quaternion& q
 130 ) const
 131 {
 132     return Foam::normalize(conjugate(*this)*q);
 133 }
 134
 135
 136 // * * * * * * * * * * * * * * * Member Operators  * * * * * * * * * * * * * //
 137
 138 inline void Foam::quaternion::operator=(const quaternion& q)
 139 {
 140     w_ = q.w_;
 141     v_ = q.v_;
 142 }
 143
 144 inline void Foam::quaternion::operator+=(const quaternion& q)
 145 {
 146     w_ += q.w_;
 147     v_ += q.v_;
 148 }
 149
 150 inline void Foam::quaternion::operator-=(const quaternion& q)
 151 {
 152     w_ -= q.w_;
 153     v_ -= q.v_;
 154 }
 155
 156 inline void Foam::quaternion::operator*=(const quaternion& q)
 157 {
 158     scalar w0 = w();
 159     w() = w()*q.w() - (v() & q.v());
 160     v() = w0*q.v() + q.w()*v() + (v() ^ q.v());
 161 }
 162
 163 inline void Foam::quaternion::operator/=(const quaternion& q)
 164 {
 165     return operator*=(inv(q));
 166 }
 167
 168
 169 inline void Foam::quaternion::operator=(const scalar s)
 170 {
 171     w_ = s;
 172 }
 173
 174
 175 inline void Foam::quaternion::operator=(const vector& v)
 176 {
 177     v_ = v;
 178 }
 179
 180
 181 inline void Foam::quaternion::operator*=(const scalar s)
 182 {
 183     w_ *= s;
 184     v_ *= s;
 185 }
 186
 187 inline void Foam::quaternion::operator/=(const scalar s)
 188 {
 189     w_ /= s;
 190     v_ /= s;
 191 }
 192
 193
 194 // * * * * * * * * * * * * * * * Global Functions  * * * * * * * * * * * * * //
 195
 196 inline Foam::scalar Foam::magSqr(const quaternion& q)
 197 {
 198     return magSqr(q.w()) + magSqr(q.v());
 199 }
 200
 201
 202 inline Foam::scalar Foam::mag(const quaternion& q)
 203 {
 204     return sqrt(magSqr(q));
 205 }
 206
 207
 208 inline Foam::quaternion Foam::conjugate(const quaternion& q)
 209 {
 210     return quaternion(q.w(), -q.v());
 211 }
 212
 213
 214 inline Foam::quaternion Foam::inv(const quaternion& q)
 215 {
 216     scalar magSqrq = magSqr(q);
 217     return quaternion(q.w()/magSqrq, -q.v()/magSqrq);
 218 }
 219
 220
 221 inline Foam::quaternion Foam::normalize(const quaternion& q)
 222 {
 223     return q/mag(q);
 224 }
 225
 226
 227 inline Foam::tensor Foam::quaternion::R() const
 228 {
 229     scalar w2 = sqr(w());
 230     scalar x2 = sqr(v().x());
 231     scalar y2 = sqr(v().y());
 232     scalar z2 = sqr(v().z());
 233
 234     scalar txy = 2*v().x()*v().y();
 235     scalar twz = 2*w()*v().z();
 236     scalar txz = 2*v().x()*v().z();
 237     scalar twy = 2*w()*v().y();
 238     scalar tyz = 2*v().y()*v().z();
 239     scalar twx = 2*w()*v().x();
 240
 241     return tensor
 242     (
 243         w2 + x2 - y2 - z2,  txy - twz,          txz + twy,
 244         txy + twz,          w2 - x2 + y2 - z2,  tyz - twx,
 245         txz - twy,          tyz + twx,          w2 - x2 - y2 + z2
 246     );
 247 }
 248
 249
 250 // * * * * * * * * * * * * * * * Global Operators  * * * * * * * * * * * * * //
 251
 252 inline bool Foam::operator==(const quaternion& q1, const quaternion& q2)
 253 {
 254     return (equal(q1.w(), q2.w()) && equal(q1.v(), q2.v()));
 255 }
 256
 257
 258 inline bool Foam::operator!=(const quaternion& q1, const quaternion& q2)
 259 {
 260     return !operator==(q1, q2);
 261 }
 262
 263
 264 inline Foam::quaternion Foam::operator+
 265 (
 266     const quaternion& q1,
 267     const quaternion& q2
 268 )
 269 {
 270     return quaternion(q1.w() + q2.w(), q1.v() + q2.v());
 271 }
 272
 273
 274 inline Foam::quaternion Foam::operator-(const quaternion& q)
 275 {
 276     return quaternion(-q.w(), -q.v());
 277 }
 278
 279
 280 inline Foam::quaternion Foam::operator-
 281 (
 282     const quaternion& q1,
 283     const quaternion& q2
 284 )
 285 {
 286     return quaternion(q1.w() - q2.w(), q1.v() - q2.v());
 287 }
 288
 289
 290 inline Foam::scalar Foam::operator&(const quaternion& q1, const quaternion& q2)
 291 {
 292     return q1.w()*q2.w() + (q1.v() & q2.v());
 293 }
 294
 295
 296 inline Foam::quaternion Foam::operator*
 297 (
 298     const quaternion& q1,
 299     const quaternion& q2
 300 )
 301 {
 302     return quaternion
 303     (
 304         q1.w()*q2.w() - (q1.v() & q2.v()),
 305         q1.w()*q2.v() + q2.w()*q1.v() + (q1.v() ^ q2.v())
 306     );
 307 }
 308
 309
 310 inline Foam::quaternion Foam::operator/
 311 (
 312     const quaternion& q1,
 313     const quaternion& q2
 314 )
 315 {
 316     return q1*inv(q2);
 317 }
 318
 319
 320 inline Foam::quaternion Foam::operator*(const scalar s, const quaternion& q)
 321 {
 322     return quaternion(s*q.w(), s*q.v());
 323 }
 324
 325
 326 inline Foam::quaternion Foam::operator*(const quaternion& q, const scalar s)
 327 {
 328     return quaternion(s*q.w(), s*q.v());
 329 }
 330
 331
 332 inline Foam::quaternion Foam::operator/(const quaternion& q, const scalar s)
 333 {
 334     return quaternion(q.w()/s, q.v()/s);
 335 }
 336
 337
 338 // ************************************************************************* //