10 #include "../utils/sh4_math.h"
35 x(0), y(0), z(0), w(1) {
48 Quaternion(
float x,
float y,
float z,
float w):
49 x(x), y(y), z(z), w(w) {
53 Euler to_euler()
const;
57 float length_squared()
const {
59 return MATH_Sum_of_Squares(x, y, z, w);
61 return (x * x + y * y + z * z) + (w * w);
65 float length()
const {
66 return sqrtf(length_squared());
71 float l = MATH_fsrra(length_squared());
73 float l = 1.0f / length();
91 float dot(
const Quaternion& rhs)
const __attribute__((always_inline)) {
93 return MATH_fipr(x, y, z, w, rhs.x, rhs.y, rhs.z, rhs.w);
95 return x * rhs.x + y * rhs.y + z * rhs.z + w * rhs.w;
100 float d = dot(*
this);
101 *
this = conjugated() / d;
111 return w == rhs.w && x == rhs.x && y == rhs.y && z == rhs.z;
114 bool operator==(
const Quaternion& rhs)
const {
115 return std::abs(dot(rhs)) > (1.0f - EPSILON);
118 bool operator!=(
const Quaternion& rhs)
const {
119 return !(*
this == rhs);
126 w = p.w * q.w - p.x * q.x - p.y * q.y - p.z * q.z;
127 x = p.w * q.x + p.x * q.w + p.y * q.z - p.z * q.y;
128 y = p.w * q.y + p.y * q.w + p.z * q.x - p.x * q.z;
129 z = p.w * q.z + p.z * q.w + p.x * q.y - p.y * q.x;
163 float l = 1.0f / rhs;
168 auto tmp1 = 1.0f - w * w;
170 return Vec3(0, 0, 1);
173 auto tmp2 = 1.0f / sqrtf(tmp1);
174 return Vec3(x * tmp2, y * tmp2, z * tmp2);
178 return Radians(std::acos(w) * 2.0f);
189 auto theta = this->dot(rhs);
197 lerp(this->x, z.x, t),
198 lerp(this->y, z.y, t),
199 lerp(this->z, z.z, t),
200 lerp(this->w, z.w, t)
207 auto cos_theta = this->dot(rhs);
210 if (cos_theta < 0.0f) {
212 cos_theta = -cos_theta;
215 const constexpr
float DOT_THRESHOLD = 0.9995f;
218 if(cos_theta > DOT_THRESHOLD) {
221 lerp(this->x, z.x, t),
222 lerp(this->y, z.y, t),
223 lerp(this->z, z.z, t),
224 lerp(this->w, z.w, t)
227 auto theta_0 = std::acos(cos_theta);
228 auto theta = theta_0 * t;
229 auto sin_theta = std::sin(theta);
230 auto sin_theta_0 = std::sin(theta_0);
233 auto s1 = MATH_Fast_Divide(sin_theta, sin_theta_0);
235 auto s1 = sin_theta / sin_theta_0;
237 auto s0 = std::cos(theta) - cos_theta * s1;
239 return ((*
this) * s0) + (z * s1);
244 return Radians(std::atan2(-2.0f * (y * z + w * x), w * w - x * x - y * y + z * z));
248 return Radians(std::asin(clamp(-2.0f * (x * z - w * y), -1.0f, 1.0f)));
252 return Radians(std::atan2(2.0f * (x * y + w * z), w * w + x * x - y * y - z * z));
255 Vec3 forward()
const {
257 return Vec3::NEGATIVE_Z.rotated_by(*
this);
261 return Vec3::POSITIVE_Y.rotated_by(*
this);
265 return Vec3::POSITIVE_X.rotated_by(*
this);
275 std::ostream& operator<<(std::ostream& stream,
const Quaternion& quat);