File: quaternion.cc

package info (click to toggle)
chromium 138.0.7204.183-1
  • links: PTS, VCS
  • area: main
  • in suites: trixie
  • size: 6,071,908 kB
  • sloc: cpp: 34,937,088; ansic: 7,176,967; javascript: 4,110,704; python: 1,419,953; asm: 946,768; xml: 739,971; pascal: 187,324; sh: 89,623; perl: 88,663; objc: 79,944; sql: 50,304; cs: 41,786; fortran: 24,137; makefile: 21,806; php: 13,980; tcl: 13,166; yacc: 8,925; ruby: 7,485; awk: 3,720; lisp: 3,096; lex: 1,327; ada: 727; jsp: 228; sed: 36
file content (132 lines) | stat: -rw-r--r-- 3,920 bytes parent folder | download | duplicates (7)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
// Copyright 2017 The Chromium Authors
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "ui/gfx/geometry/quaternion.h"

#include <algorithm>
#include <cmath>
#include <numbers>

#include "base/strings/stringprintf.h"
#include "ui/gfx/geometry/vector3d_f.h"

namespace gfx {

namespace {

const double kEpsilon = 1e-5;

}  // namespace

Quaternion::Quaternion(const Vector3dF& axis, double theta) {
  // Rotation angle is the product of |angle| and the magnitude of |axis|.
  double length = axis.Length();
  if (std::abs(length) < kEpsilon)
    return;

  Vector3dF normalized = axis;
  normalized.InvScale(length);

  theta *= 0.5;
  double s = sin(theta);
  x_ = normalized.x() * s;
  y_ = normalized.y() * s;
  z_ = normalized.z() * s;
  w_ = cos(theta);
}

Quaternion::Quaternion(const Vector3dF& from, const Vector3dF& to) {
  double dot = gfx::DotProduct(from, to);
  double norm = sqrt(from.LengthSquared() * to.LengthSquared());
  double real = norm + dot;
  gfx::Vector3dF axis;
  if (real < kEpsilon * norm) {
    real = 0.0f;
    axis = std::abs(from.x()) > std::abs(from.z())
               ? gfx::Vector3dF{-from.y(), from.x(), 0.0}
               : gfx::Vector3dF{0.0, -from.z(), from.y()};
  } else {
    axis = gfx::CrossProduct(from, to);
  }
  x_ = axis.x();
  y_ = axis.y();
  z_ = axis.z();
  w_ = real;
  *this = this->Normalized();
}

Quaternion Quaternion::FromAxisAngle(double x,
                                     double y,
                                     double z,
                                     double angle) {
  double length = std::sqrt(x * x + y * y + z * z);
  if (std::abs(length) < kEpsilon)
    return Quaternion(0, 0, 0, 1);

  double scale = std::sin(0.5 * angle) / length;
  return Quaternion(scale * x, scale * y, scale * z, std::cos(0.5 * angle));
}

// Adapted from https://www.euclideanspace.com/maths/algebra/realNormedAlgebra/
// quaternions/slerp/index.htm
Quaternion Quaternion::Slerp(const Quaternion& to, double t) const {
  Quaternion from = *this;

  double cos_half_angle =
      from.x_ * to.x_ + from.y_ * to.y_ + from.z_ * to.z_ + from.w_ * to.w_;
  if (cos_half_angle < 0) {
    // Since the half angle is > 90 degrees, the full rotation angle would
    // exceed 180 degrees. The quaternions (x, y, z, w) and (-x, -y, -z, -w)
    // represent the same rotation. Flipping the orientation of either
    // quaternion ensures that the half angle is less than 90 and that we are
    // taking the shortest path.
    from = from.flip();
    cos_half_angle = -cos_half_angle;
  }

  // Ensure that acos is well behaved at the boundary.
  if (cos_half_angle > 1)
    cos_half_angle = 1;

  double sin_half_angle = std::sqrt(1.0 - cos_half_angle * cos_half_angle);
  if (sin_half_angle < kEpsilon) {
    // Quaternions share common axis and angle.
    return *this;
  }

  double half_angle = std::acos(cos_half_angle);

  double scaleA = std::sin((1 - t) * half_angle) / sin_half_angle;
  double scaleB = std::sin(t * half_angle) / sin_half_angle;

  return (scaleA * from) + (scaleB * to);
}

Quaternion Quaternion::Lerp(const Quaternion& q, double t) const {
  return (((1.0 - t) * *this) + (t * q)).Normalized();
}

double Quaternion::Length() const {
  return x_ * x_ + y_ * y_ + z_ * z_ + w_ * w_;
}

Quaternion Quaternion::Normalized() const {
  double length = Length();
  if (length < kEpsilon)
    return *this;
  return *this / sqrt(length);
}

std::string Quaternion::ToString() const {
  // q = (con(abs(v_theta)/2), v_theta/abs(v_theta) * sin(abs(v_theta)/2))
  float abs_theta = acos(w_) * 2;
  float scale = 1. / sin(abs_theta * .5);
  gfx::Vector3dF v(x_, y_, z_);
  v.Scale(scale);
  return base::StringPrintf("[%f %f %f %f], v:", x_, y_, z_, w_) +
         v.ToString() +
         base::StringPrintf(", θ:%fπ", abs_theta / std::numbers::pi_v<float>);
}

}  // namespace gfx