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
|
#ifndef MATH_QUAT_HPP
#define MATH_QUAT_HPP
#include <cmath>
#include "math/vector.hpp"
namespace engine::math {
struct Quaternion {
static constexpr Quaternion zero() {
return {0.f, 0.f, 0.f, 0.f};
}
static constexpr Quaternion one() {
return {1.f, 0.f, 0.f, 0.f};
}
static constexpr Quaternion euler_zxy(float rx, float ry, float rz) {
float ca = std::cos(rx / 2.f), sa = std::sin(rx / 2.f),
cb = std::cos(ry / 2.f), sb = std::sin(ry / 2.f),
cc = std::cos(rz / 2.f), sc = std::sin(rz / 2.f);
return {
ca * cb * cc + sa * sb * sc,
sa * cb * cc + ca * sb * sc,
ca * sb * cc - sa * cb * sc,
ca * cb * sc - sa * sb * cc,
};
}
static constexpr Quaternion rot_y(float a) {
return {std::cos(a / 2.f), 0.f, std::sin(a / 2.f), 0.f};
}
static constexpr Quaternion look_towards(const Vector3& dir, const Vector3& up) {
// TODO: extract common code between Matrix4::look_at and this. We should have something
// similar to a function returning a 3x3 matrix which does:
// e_x -> up.cross(-dir).normalize()
// e_y -> (-dir).cross(e_x).normalize()
// e_z -> (-dir).normalize()
Vector3 new_x = up.cross(-dir).normalize();
Vector3 new_y = (-dir).cross(new_x).normalize();
Vector3 new_z = (-dir).normalize();
return {
std::sqrt(std::max(0.f, new_x.x + new_y.y + new_z.z + 1.f)) / 2.f,
std::copysign(std::sqrt(std::max(0.f, new_x.x - new_y.y - new_z.z + 1.f)) / 2.f, new_y.z - new_z.y),
std::copysign(std::sqrt(std::max(0.f, -new_x.x + new_y.y - new_z.z + 1.f)) / 2.f, new_z.x - new_x.z),
std::copysign(std::sqrt(std::max(0.f, -new_x.x - new_y.y + new_z.z + 1.f)) / 2.f, new_x.y - new_y.x),
};
}
float w, x, y, z;
constexpr Quaternion() {}
constexpr Quaternion(float w, float x, float y, float z) : w{w}, x{x}, y{y}, z{z} {}
constexpr bool operator==(const Quaternion& other) const & {
return w == other.w && x == other.x && y == other.y && z == other.z;
}
constexpr bool operator!=(const Quaternion& other) const & {
return !(*this == other);
}
constexpr Quaternion operator+() const & {
return *this;
}
constexpr Quaternion operator-() const & {
return { -w, -x, -y, -z };
}
constexpr Quaternion operator+(const Quaternion& other) const & {
return { w + other.w, x + other.x, y + other.y, z + other.z };
}
constexpr Quaternion operator-(const Quaternion& other) const & {
return *this + (-other);
}
constexpr Quaternion operator*(const Quaternion& other) const & {
return {
w * other.w - x * other.x - y * other.y - z * other.z,
w * other.x + x * other.w + y * other.z - z * other.y,
w * other.y + y * other.w + z * other.x - x * other.z,
w * other.z + z * other.w + x * other.y - y * other.x,
};
}
constexpr Quaternion conjugate() const & {
return {w, -x, -y, -z};
}
constexpr Vector3 rot(const Vector3& v) const & {
return {
(2.f * (w * w + x * x) - 1.f) * v.x + (2.f * (x * y - w * z) ) * v.y + (2.f * (x * z + w * y) ) * v.z,
(2.f * (x * y + w * z) ) * v.x + (2.f * (w * w + y * y) - 1.f) * v.y + (2.f * (y * z - w * x) ) * v.z,
(2.f * (x * z - w * y) ) * v.x + (2.f * (y * z + w * x) ) * v.y + (2.f * (w * w + z * z) - 1.f) * v.z,
};
}
};
}
#endif // MATH_QUAT_HPP
|
