1#pragma once
2
3#ifndef SEAD_MATH_QUAT_CALC_COMMON_H_
4#include <math/seadQuatCalcCommon.h>
5#endif
6
7#include <limits>
8#include <math/seadMathCalcCommon.h>
9#include <math/seadVectorCalcCommon.h>
10
11namespace sead
12{
13template <typename T>
14inline T QuatCalcCommon<T>::length(const Base& v)
15{
16 return std::sqrt(dot(u: v, v));
17}
18
19template <typename T>
20inline T QuatCalcCommon<T>::normalize(Base& v)
21{
22 const T len = length(v);
23 if (len > 0)
24 {
25 const T inv_len = 1 / len;
26 v.w *= inv_len;
27 v.x *= inv_len;
28 v.y *= inv_len;
29 v.z *= inv_len;
30 }
31
32 return len;
33}
34
35template <typename T>
36inline T QuatCalcCommon<T>::dot(const Base& u, const Base& v)
37{
38 return (u.w * v.w) + (u.x * v.x) + (u.y * v.y) + (u.z * v.z);
39}
40
41template <typename T>
42inline void QuatCalcCommon<T>::setMul(Base& out, const Base& u, const Base& v)
43{
44 T w = (u.w * v.w) - (u.x * v.x) - (u.y * v.y) - (u.z * v.z);
45 T x = (u.w * v.x) + (u.x * v.w) + (u.y * v.z) - (u.z * v.y);
46 T y = (u.w * v.y) - (u.x * v.z) + (u.y * v.w) + (u.z * v.x);
47 T z = (u.w * v.z) + (u.x * v.y) - (u.y * v.x) + (u.z * v.w);
48 out.w = w;
49 out.x = x;
50 out.y = y;
51 out.z = z;
52}
53
54template <typename T>
55inline void QuatCalcCommon<T>::slerpTo(Base& out, const Base& q1, const Base& q2, f32 t)
56{
57 T dot = (q1.x * q2.x) + (q1.y * q2.y) + (q1.z * q2.z) + (q1.w * q2.w);
58 if (dot > 1)
59 dot = 1;
60 else if (dot < -1)
61 dot = -1;
62
63 const T dot_0 = dot;
64 if (dot < 0)
65 dot = -dot;
66
67 T a, b;
68 const T theta_0 = MathCalcCommon<T>::acos(dot);
69 const T sin_theta_0 = std::sin(theta_0);
70 if (MathCalcCommon<T>::abs(sin_theta_0) < std::numeric_limits<T>::epsilon())
71 {
72 a = 1.0f - t;
73 b = t;
74 }
75 else
76 {
77 const T theta = theta_0 * t;
78 const T sin_theta_0_inv = 1.0f / sin_theta_0;
79 a = sin_theta_0_inv * std::sin(theta_0 - theta);
80 b = sin_theta_0_inv * std::sin(theta);
81 }
82
83 if (dot_0 < 0)
84 b = -b;
85
86 out.x = a * q1.x + b * q2.x;
87 out.y = a * q1.y + b * q2.y;
88 out.z = a * q1.z + b * q2.z;
89 out.w = a * q1.w + b * q2.w;
90}
91
92template <typename T>
93inline void QuatCalcCommon<T>::makeUnit(Base& q)
94{
95 q = {0, 0, 0, 1};
96}
97
98template <typename T>
99inline bool QuatCalcCommon<T>::makeVectorRotation(Base& q, const Vec3& from, const Vec3& to)
100{
101 // Based on The Shortest Arc Quaternion from Game Programming Gems 1
102 Vec3 cross;
103 Vector3CalcCommon<T>::cross(cross, from, to);
104 const T dot = Vector3CalcCommon<T>::dot(from, to) + 1;
105
106 if (dot <= MathCalcCommon<T>::epsilon())
107 {
108 makeUnit(q);
109 return false;
110 }
111 else
112 {
113 T s = MathCalcCommon<T>::sqrt(2 * dot);
114 T rs = 1 / s;
115 set(q, s * 0.5f, cross.x * rs, cross.y * rs, cross.z * rs);
116 return true;
117 }
118}
119
120template <typename T>
121inline void QuatCalcCommon<T>::set(Base& q, const Base& other)
122{
123 q = other;
124}
125
126template <typename T>
127inline void QuatCalcCommon<T>::set(Base& q, T w, T x, T y, T z)
128{
129 q.w = w;
130 q.x = x;
131 q.y = y;
132 q.z = z;
133}
134
135template <typename T>
136inline void QuatCalcCommon<T>::setRPY(Base& q, T roll, T pitch, T yaw)
137{
138 const T cy = std::cos(yaw / 2);
139 const T cp = std::cos(pitch / 2);
140 const T cr = std::cos(roll / 2);
141 const T sy = std::sin(yaw / 2);
142 const T sp = std::sin(pitch / 2);
143 const T sr = std::sin(roll / 2);
144
145 const T cy_cp = cy * cp;
146 const T sy_sp = sy * sp;
147 const T cy_sp = cy * sp;
148 const T sy_cp = sy * cp;
149
150 q.w = (cy_cp * cr) + (sy_sp * sr);
151 q.x = (cy_cp * sr) - (sy_sp * cr);
152 q.y = (cy_sp * cr) + (sy_cp * sr);
153 q.z = (sy_cp * cr) - (cy_sp * sr);
154}
155
156template <typename T>
157inline void QuatCalcCommon<T>::setAxisAngle(Base& q, const Vec3& axis, T angle)
158{
159 T angleRad = MathCalcCommon<T>::deg2rad(angle);
160 angleRad *= 0.5f;
161 q.w = MathCalcCommon<T>::cos(angleRad);
162 T sa = MathCalcCommon<T>::sin(angleRad);
163 q.x = sa * axis.x;
164 q.y = sa * axis.y;
165 q.z = sa * axis.z;
166}
167
168} // namespace sead
169