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
|
// Copyright 2017 The Chromium Authors. All rights reserved.
// 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 "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.Scale(1.0 / 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();
}
// Taken from http://www.w3.org/TR/css3-transforms/.
Quaternion Quaternion::Slerp(const Quaternion& q, double t) const {
double dot = x_ * q.x_ + y_ * q.y_ + z_ * q.z_ + w_ * q.w_;
// Clamp dot to -1.0 <= dot <= 1.0.
dot = std::min(std::max(dot, -1.0), 1.0);
// Quaternions are facing the same direction.
if (std::abs(dot - 1.0) < kEpsilon || std::abs(dot + 1.0) < kEpsilon)
return *this;
double denom = std::sqrt(1.0 - dot * dot);
double theta = std::acos(dot);
double w = std::sin(t * theta) * (1.0 / denom);
double s1 = std::cos(t * theta) - dot * w;
double s2 = w;
return (s1 * *this) + (s2 * q);
}
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 {
return base::StringPrintf("[%f %f %f %f]", x_, y_, z_, w_);
}
} // namespace gfx
|
