quaternion.cc 3.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132
  1. // Copyright 2017 The Chromium Authors. All rights reserved.
  2. // Use of this source code is governed by a BSD-style license that can be
  3. // found in the LICENSE file.
  4. #include "ui/gfx/geometry/quaternion.h"
  5. #include <algorithm>
  6. #include <cmath>
  7. #include "base/numerics/math_constants.h"
  8. #include "base/strings/stringprintf.h"
  9. #include "ui/gfx/geometry/vector3d_f.h"
  10. namespace gfx {
  11. namespace {
  12. const double kEpsilon = 1e-5;
  13. } // namespace
  14. Quaternion::Quaternion(const Vector3dF& axis, double theta) {
  15. // Rotation angle is the product of |angle| and the magnitude of |axis|.
  16. double length = axis.Length();
  17. if (std::abs(length) < kEpsilon)
  18. return;
  19. Vector3dF normalized = axis;
  20. normalized.Scale(1.0 / length);
  21. theta *= 0.5;
  22. double s = sin(theta);
  23. x_ = normalized.x() * s;
  24. y_ = normalized.y() * s;
  25. z_ = normalized.z() * s;
  26. w_ = cos(theta);
  27. }
  28. Quaternion::Quaternion(const Vector3dF& from, const Vector3dF& to) {
  29. double dot = gfx::DotProduct(from, to);
  30. double norm = sqrt(from.LengthSquared() * to.LengthSquared());
  31. double real = norm + dot;
  32. gfx::Vector3dF axis;
  33. if (real < kEpsilon * norm) {
  34. real = 0.0f;
  35. axis = std::abs(from.x()) > std::abs(from.z())
  36. ? gfx::Vector3dF{-from.y(), from.x(), 0.0}
  37. : gfx::Vector3dF{0.0, -from.z(), from.y()};
  38. } else {
  39. axis = gfx::CrossProduct(from, to);
  40. }
  41. x_ = axis.x();
  42. y_ = axis.y();
  43. z_ = axis.z();
  44. w_ = real;
  45. *this = this->Normalized();
  46. }
  47. Quaternion Quaternion::FromAxisAngle(double x,
  48. double y,
  49. double z,
  50. double angle) {
  51. double length = std::sqrt(x * x + y * y + z * z);
  52. if (std::abs(length) < kEpsilon)
  53. return Quaternion(0, 0, 0, 1);
  54. double scale = std::sin(0.5 * angle) / length;
  55. return Quaternion(scale * x, scale * y, scale * z, std::cos(0.5 * angle));
  56. }
  57. // Adapted from https://www.euclideanspace.com/maths/algebra/realNormedAlgebra/
  58. // quaternions/slerp/index.htm
  59. Quaternion Quaternion::Slerp(const Quaternion& to, double t) const {
  60. Quaternion from = *this;
  61. double cos_half_angle =
  62. from.x_ * to.x_ + from.y_ * to.y_ + from.z_ * to.z_ + from.w_ * to.w_;
  63. if (cos_half_angle < 0) {
  64. // Since the half angle is > 90 degrees, the full rotation angle would
  65. // exceed 180 degrees. The quaternions (x, y, z, w) and (-x, -y, -z, -w)
  66. // represent the same rotation. Flipping the orientation of either
  67. // quaternion ensures that the half angle is less than 90 and that we are
  68. // taking the shortest path.
  69. from = from.flip();
  70. cos_half_angle = -cos_half_angle;
  71. }
  72. // Ensure that acos is well behaved at the boundary.
  73. if (cos_half_angle > 1)
  74. cos_half_angle = 1;
  75. double sin_half_angle = std::sqrt(1.0 - cos_half_angle * cos_half_angle);
  76. if (sin_half_angle < kEpsilon) {
  77. // Quaternions share common axis and angle.
  78. return *this;
  79. }
  80. double half_angle = std::acos(cos_half_angle);
  81. double scaleA = std::sin((1 - t) * half_angle) / sin_half_angle;
  82. double scaleB = std::sin(t * half_angle) / sin_half_angle;
  83. return (scaleA * from) + (scaleB * to);
  84. }
  85. Quaternion Quaternion::Lerp(const Quaternion& q, double t) const {
  86. return (((1.0 - t) * *this) + (t * q)).Normalized();
  87. }
  88. double Quaternion::Length() const {
  89. return x_ * x_ + y_ * y_ + z_ * z_ + w_ * w_;
  90. }
  91. Quaternion Quaternion::Normalized() const {
  92. double length = Length();
  93. if (length < kEpsilon)
  94. return *this;
  95. return *this / sqrt(length);
  96. }
  97. std::string Quaternion::ToString() const {
  98. // q = (con(abs(v_theta)/2), v_theta/abs(v_theta) * sin(abs(v_theta)/2))
  99. float abs_theta = acos(w_) * 2;
  100. float scale = 1. / sin(abs_theta * .5);
  101. gfx::Vector3dF v(x_, y_, z_);
  102. v.Scale(scale);
  103. return base::StringPrintf("[%f %f %f %f], v:", x_, y_, z_, w_) +
  104. v.ToString() +
  105. base::StringPrintf(", θ:%fπ", abs_theta / base::kPiFloat);
  106. }
  107. } // namespace gfx