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