Simulant  21.09-46
A portable game engine for Windows, OSX, Linux, Dreamcast, and PSP
quaternion.h
1 #pragma once
2 
3 #include "degrees.h"
4 #include "radians.h"
5 #include "euler.h"
6 #include "utils.h"
7 #include "vec3.h"
8 
9 #ifdef __DREAMCAST__
10 #include "../utils/sh4_math.h"
11 #endif
12 
13 namespace smlt {
14 
15 struct Vec3;
16 struct Mat3;
17 struct Mat4;
18 
19 struct AxisAngle {
20  Vec3 axis;
21  Degrees angle;
22 };
23 
24 struct Quaternion {
25  friend struct Vec3;
26  friend struct Mat4;
27  friend struct Mat3;
28 
29  float x;
30  float y;
31  float z;
32  float w;
33 
34  Quaternion():
35  x(0), y(0), z(0), w(1) {
36 
37  }
38 
39  Quaternion(const Degrees& pitch, const Degrees& yaw, const Degrees& roll);
40 
41  Quaternion(const Vec3& axis, const Degrees& degrees);
42  Quaternion(const Mat3& rot_matrix);
43 
44  Quaternion(const Euler& angles) : Quaternion(Degrees(angles.x), Degrees(angles.y), Degrees(angles.z)) {
45 
46  }
47 
48  Quaternion(float x, float y, float z, float w):
49  x(x), y(y), z(z), w(w) {
50 
51  }
52 
53  Euler to_euler() const;
54 
55  AxisAngle to_axis_angle() const;
56 
57  float length_squared() const {
58 #ifdef __DREAMCAST__
59  return MATH_Sum_of_Squares(x, y, z, w);
60 #else
61  return (x * x + y * y + z * z) + (w * w);
62 #endif
63  }
64 
65  float length() const {
66  return sqrtf(length_squared());
67  }
68 
69  void normalize() {
70 #ifdef __DREAMCAST__
71  float l = MATH_fsrra(length_squared());
72 #else
73  float l = 1.0f / length();
74 #endif
75  x *= l;
76  y *= l;
77  z *= l;
78  w *= l;
79  }
80 
81  const Quaternion normalized() {
82  Quaternion result = *this;
83  result.normalize();
84  return result;
85  }
86 
87  Quaternion conjugated() {
88  return Quaternion(-x, -y, -z, w);
89  }
90 
91  float dot(const Quaternion& rhs) const __attribute__((always_inline)) {
92 #ifdef __DREAMCAST__
93  return MATH_fipr(x, y, z, w, rhs.x, rhs.y, rhs.z, rhs.w);
94 #else
95  return x * rhs.x + y * rhs.y + z * rhs.z + w * rhs.w;
96 #endif
97  }
98 
99  void inverse() {
100  float d = dot(*this);
101  *this = conjugated() / d;
102  }
103 
104  const Quaternion inversed() const {
105  Quaternion result(*this);
106  result.inverse();
107  return result;
108  }
109 
110  bool equals(const Quaternion& rhs) const {
111  return w == rhs.w && x == rhs.x && y == rhs.y && z == rhs.z;
112  }
113 
114  bool operator==(const Quaternion& rhs) const {
115  return std::abs(dot(rhs)) > (1.0f - EPSILON);
116  }
117 
118  bool operator!=(const Quaternion& rhs) const {
119  return !(*this == rhs);
120  }
121 
122  Quaternion& operator*=(const Quaternion& rhs) {
123  const Quaternion p(*this);
124  const Quaternion q(rhs);
125 
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;
130  return *this;
131  }
132 
133  Quaternion operator*(const Quaternion& rhs) const {
134  return Quaternion(*this) *= rhs;
135  }
136 
137  Quaternion& operator+=(const Quaternion& rhs) {
138  x += rhs.x;
139  y += rhs.y;
140  z += rhs.z;
141  w += rhs.w;
142  return *this;
143  }
144 
145  Quaternion operator+(const Quaternion& rhs) const {
146  return Quaternion(*this) += rhs;
147  }
148 
149  Quaternion& operator*=(const float rhs) {
150  x *= rhs;
151  y *= rhs;
152  z *= rhs;
153  w *= rhs;
154 
155  return *this;
156  }
157 
158  Quaternion operator*(const float rhs) const {
159  return Quaternion(*this) *= rhs;
160  }
161 
162  Quaternion operator/(const float rhs) const {
163  float l = 1.0f / rhs;
164  return Quaternion(*this) *= l;
165  }
166 
167  Vec3 axis() const {
168  auto tmp1 = 1.0f - w * w;
169  if(tmp1 <= 0.0f) {
170  return Vec3(0, 0, 1);
171  }
172 
173  auto tmp2 = 1.0f / sqrtf(tmp1);
174  return Vec3(x * tmp2, y * tmp2, z * tmp2);
175  }
176 
177  Radians angle() const {
178  return Radians(std::acos(w) * 2.0f);
179  }
180 
181  Quaternion operator-() const {
182  return Quaternion(
183  -x, -y, -z, -w
184  );
185  }
186 
187  Quaternion nlerp(const Quaternion& rhs, float t) const {
188  auto z = rhs;
189  auto theta = this->dot(rhs);
190 
191  if(theta < 0.0f) {
192  z = -rhs;
193  }
194 
195  // Linear interpolation (result normalized)
196  return Quaternion(
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)
201  ).normalized();
202  }
203 
204  Quaternion slerp(const Quaternion& rhs, float t) const {
205  auto z = rhs;
206 
207  auto cos_theta = this->dot(rhs);
208 
209  // negate to avoid interpolation taking long way around
210  if (cos_theta < 0.0f) {
211  z = -rhs;
212  cos_theta = -cos_theta;
213  }
214 
215  const constexpr float DOT_THRESHOLD = 0.9995f;
216 
217  // Lerp to avoid side effect of sin(angle) becoming a zero denominator
218  if(cos_theta > DOT_THRESHOLD) {
219  // Linear interpolation
220  return Quaternion(
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)
225  ).normalized();
226  } else {
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);
231 
232 #ifdef __DREAMCAST__
233  auto s1 = MATH_Fast_Divide(sin_theta, sin_theta_0);
234 #else
235  auto s1 = sin_theta / sin_theta_0;
236 #endif
237  auto s0 = std::cos(theta) - cos_theta * s1;
238 
239  return ((*this) * s0) + (z * s1);
240  }
241  }
242 
243  const Degrees pitch() const {
244  return Radians(std::atan2(-2.0f * (y * z + w * x), w * w - x * x - y * y + z * z));
245  }
246 
247  const Degrees yaw() const {
248  return Radians(std::asin(clamp(-2.0f * (x * z - w * y), -1.0f, 1.0f)));
249  }
250 
251  const Degrees roll() const {
252  return Radians(std::atan2(2.0f * (x * y + w * z), w * w + x * x - y * y - z * z));
253  }
254 
255  Vec3 forward() const {
256  // OpenGL coordinate system has Neg-z as "forward"
257  return Vec3::NEGATIVE_Z.rotated_by(*this);
258  }
259 
260  Vec3 up() const {
261  return Vec3::POSITIVE_Y.rotated_by(*this);
262  }
263 
264  Vec3 right() const {
265  return Vec3::POSITIVE_X.rotated_by(*this);
266  }
267 
268  /* Returns the Quaternion rotation representing a turn to direction, using up as a basis.
269  * If up and direction are colinear, or either are zero length, returns an identity
270  * Quaternion */
271  static Quaternion look_rotation(const Vec3& direction, const Vec3& up=Vec3(0, 1, 0));
272 };
273 
274 Quaternion operator*(float s, const Quaternion& q);
275 std::ostream& operator<<(std::ostream& stream, const Quaternion& quat);
276 
277 }
smlt::Mat4
Definition: mat4.h:25
smlt::AxisAngle
Definition: quaternion.h:19
smlt::Vec3
Definition: vec3.h:23
smlt::Quaternion
Definition: quaternion.h:24
smlt::Radians
Definition: radians.h:7
smlt::Mat3
Definition: mat3.h:13
smlt
Definition: animation.cpp:25
smlt::Degrees
Definition: degrees.h:7
smlt::Euler
Definition: euler.h:7