/src/OpenFOAM/primitives/quaternion/quaternion.C

https://gitlab.com/johnvarv/OpenFOAM-3.0.x · C · 154 lines · 83 code · 39 blank · 32 comment · 11 complexity · 9875b3676d2d2f9ef5ec994ec8ca181f MD5 · raw file

  1. /*---------------------------------------------------------------------------*\
  2. ========= |
  3. \\ / F ield | OpenFOAM: The Open Source CFD Toolbox
  4. \\ / O peration |
  5. \\ / A nd | Copyright (C) 2011-2013 OpenFOAM Foundation
  6. \\/ M anipulation |
  7. -------------------------------------------------------------------------------
  8. License
  9. This file is part of OpenFOAM.
  10. OpenFOAM is free software: you can redistribute it and/or modify it
  11. under the terms of the GNU General Public License as published by
  12. the Free Software Foundation, either version 3 of the License, or
  13. (at your option) any later version.
  14. OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
  15. ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  16. FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  17. for more details.
  18. You should have received a copy of the GNU General Public License
  19. along with OpenFOAM. If not, see <http://www.gnu.org/licenses/>.
  20. \*---------------------------------------------------------------------------*/
  21. #include "quaternion.H"
  22. #include "IOstreams.H"
  23. #include "OStringStream.H"
  24. // * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
  25. const char* const Foam::quaternion::typeName = "quaternion";
  26. const Foam::quaternion Foam::quaternion::zero(0, vector(0, 0, 0));
  27. const Foam::quaternion Foam::quaternion::I(1, vector(0, 0, 0));
  28. // * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
  29. Foam::quaternion::quaternion(Istream& is)
  30. {
  31. is >> *this;
  32. }
  33. // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
  34. Foam::word Foam::name(const quaternion& q)
  35. {
  36. OStringStream buf;
  37. buf << '(' << q.w() << ',' << q.v() << ')';
  38. return buf.str();
  39. }
  40. Foam::quaternion Foam::slerp
  41. (
  42. const quaternion& qa,
  43. const quaternion& qb,
  44. const scalar t
  45. )
  46. {
  47. label sign = 1;
  48. if ((qa & qb) < 0)
  49. {
  50. sign = -1;
  51. }
  52. return qa*pow((inv(qa)*sign*qb), t);
  53. }
  54. Foam::quaternion Foam::exp(const quaternion& q)
  55. {
  56. const scalar magV = mag(q.v());
  57. if (magV == 0)
  58. {
  59. return quaternion(1, vector::zero);
  60. }
  61. const scalar expW = exp(q.w());
  62. return quaternion
  63. (
  64. expW*cos(magV),
  65. expW*sin(magV)*q.v()/magV
  66. );
  67. }
  68. Foam::quaternion Foam::pow(const quaternion& q, const label power)
  69. {
  70. const scalar magQ = mag(q);
  71. const scalar magV = mag(q.v());
  72. quaternion powq(q.v());
  73. if (magV != 0 && magQ != 0)
  74. {
  75. powq /= magV;
  76. powq *= power*acos(q.w()/magQ);
  77. }
  78. return pow(magQ, power)*exp(powq);
  79. }
  80. Foam::quaternion Foam::pow(const quaternion& q, const scalar power)
  81. {
  82. const scalar magQ = mag(q);
  83. const scalar magV = mag(q.v());
  84. quaternion powq(q.v());
  85. if (magV != 0 && magQ != 0)
  86. {
  87. powq /= magV;
  88. powq *= power*acos(q.w()/magQ);
  89. }
  90. return pow(magQ, power)*exp(powq);
  91. }
  92. // * * * * * * * * * * * * * * * IOstream Operators * * * * * * * * * * * * //
  93. Foam::Istream& Foam::operator>>(Istream& is, quaternion& q)
  94. {
  95. // Read beginning of quaternion
  96. is.readBegin("quaternion");
  97. is >> q.w() >> q.v();
  98. // Read end of quaternion
  99. is.readEnd("quaternion");
  100. // Check state of Istream
  101. is.check("operator>>(Istream&, quaternion&)");
  102. return is;
  103. }
  104. Foam::Ostream& Foam::operator<<(Ostream& os, const quaternion& q)
  105. {
  106. os << token::BEGIN_LIST
  107. << q.w() << token::SPACE << q.v()
  108. << token::END_LIST;
  109. return os;
  110. }
  111. // ************************************************************************* //