1 module gfm.math.quaternion;
2
3 import std.math,
4 std..string;
5
6 import gfm.math.vector,
7 gfm.math.matrix,
8 funcs = gfm.math.funcs;
9
10 /// Quaternion implementation.
11 /// Holds a rotation + angle in a proper but wild space.
12 struct Quaternion(T)
13 {
14 public
15 {
16 union
17 {
18 Vector!(T, 4u) v;
19 struct
20 {
21 T x, y, z, w;
22 }
23 }
24
25 /// Construct a Quaternion from a value.
26 @nogc this(U)(U x) pure nothrow if (isAssignable!U)
27 {
28 opAssign!U(x);
29 }
30
31 /// Constructs a Quaternion from coordinates.
32 /// Warning: order of coordinates is different from storage.
33 @nogc this(T qw, T qx, T qy, T qz) pure nothrow
34 {
35 x = qx;
36 y = qy;
37 z = qz;
38 w = qw;
39 }
40
41 /// Constructs a Quaternion from axis + angle.
42 @nogc static Quaternion fromAxis(Vector!(T, 3) axis, T angle) pure nothrow
43 {
44 Quaternion q = void;
45 axis.normalize();
46 T cos_a = cos(angle / 2);
47 T sin_a = sin(angle / 2);
48 q.x = sin_a * axis.x;
49 q.y = sin_a * axis.y;
50 q.z = sin_a * axis.z;
51 q.w = cos_a;
52 return q; // should be normalized
53 }
54
55 /// Constructs a Quaternion from Euler angles.
56 /// All paramers given in radians.
57 /// Roll->X axis, Pitch->Y axis, Yaw->Z axis
58 /// See_also: $(LINK https://www.cs.princeton.edu/~gewang/projects/darth/stuff/quat_faq.html)
59 @nogc static Quaternion fromEulerAngles(T roll, T pitch, T yaw) pure nothrow
60 {
61 Quaternion q = void;
62 T sinPitch = sin(pitch / 2);
63 T cosPitch = cos(pitch / 2);
64 T sinYaw = sin(yaw / 2);
65 T cosYaw = cos(yaw / 2);
66 T sinRoll = sin(roll / 2);
67 T cosRoll = cos(roll / 2);
68 T cosPitchCosYaw = cosPitch * cosYaw;
69 T sinPitchSinYaw = sinPitch * sinYaw;
70 q.x = sinRoll * cosPitchCosYaw - cosRoll * sinPitchSinYaw;
71 q.y = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
72 q.z = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
73 q.w = cosRoll * cosPitchCosYaw + sinRoll * sinPitchSinYaw;
74 return q;
75 }
76
77 /// Converts a quaternion to Euler angles.
78 /// TODO: adds a EulerAngles type.
79 /// Returns: A vector which contains roll-pitch-yaw as x-y-z.
80 @nogc vec3!T toEulerAngles() pure const nothrow
81 {
82 mat3!T m = cast(mat3!T)(this);
83
84 T pitch, yaw, roll;
85 T s = sqrt(m.c[0][0] * m.c[0][0] + m.c[1][0] * m.c[1][0]);
86 if (s > T.epsilon)
87 {
88 pitch = - atan2(m.c[2][0], s);
89 yaw = atan2(m.c[1][0], m.c[0][0]);
90 roll = atan2(m.c[2][1], m.c[2][2]);
91 }
92 else
93 {
94 pitch = m.c[2][0] < 0.0f ? PI /2 : -PI / 2;
95 yaw = -atan2(m.c[0][1], m.c[1][1]);
96 roll = 0.0f;
97 }
98 return vec3!T(roll, pitch, yaw);
99 }
100
101 /// Assign from another Quaternion.
102 @nogc ref Quaternion opAssign(U)(U u) pure nothrow if (is(typeof(U._isQuaternion)) && is(U._T : T))
103 {
104 v = u.v;
105 return this;
106 }
107
108 /// Assign from a vector of 4 elements.
109 @nogc ref Quaternion opAssign(U)(U u) pure nothrow if (is(U : Vector!(T, 4u)))
110 {
111 v = u;
112 return this;
113 }
114
115 /// Converts to a pretty string.
116 string toString() const nothrow
117 {
118 try
119 return format("%s", v);
120 catch (Exception e)
121 assert(false); // should not happen since format is right
122 }
123
124 /// Normalizes a quaternion.
125 @nogc void normalize() pure nothrow
126 {
127 v.normalize();
128 }
129
130 /// Returns: Normalized quaternion.
131 @nogc Quaternion normalized() pure const nothrow
132 {
133 Quaternion res = void;
134 res.v = v.normalized();
135 return res;
136 }
137
138 /// Inverses a quaternion in-place.
139 @nogc void inverse() pure nothrow
140 {
141 x = -x;
142 y = -y;
143 z = -z;
144 }
145
146 /// Returns: Inverse of quaternion.
147 @nogc Quaternion inversed() pure const nothrow
148 {
149 Quaternion res = void;
150 res.v = v;
151 res.inverse();
152 return res;
153 }
154
155 @nogc ref Quaternion opOpAssign(string op, U)(U q) pure nothrow
156 if (is(U : Quaternion) && (op == "*"))
157 {
158 T nx = w * q.x + x * q.w + y * q.z - z * q.y,
159 ny = w * q.y + y * q.w + z * q.x - x * q.z,
160 nz = w * q.z + z * q.w + x * q.y - y * q.x,
161 nw = w * q.w - x * q.x - y * q.y - z * q.z;
162 x = nx;
163 y = ny;
164 z = nz;
165 w = nw;
166 return this;
167 }
168
169 @nogc ref Quaternion opOpAssign(string op, U)(U operand) pure nothrow if (isConvertible!U)
170 {
171 Quaternion conv = operand;
172 return opOpAssign!op(conv);
173 }
174
175 @nogc Quaternion opBinary(string op, U)(U operand) pure const nothrow
176 if (is(U: Quaternion) || (isConvertible!U))
177 {
178 Quaternion temp = this;
179 return temp.opOpAssign!op(operand);
180 }
181
182 /// Compare two Quaternions.
183 bool opEquals(U)(U other) pure const if (is(U : Quaternion))
184 {
185 return v == other.v;
186 }
187
188 /// Compare Quaternion and other types.
189 bool opEquals(U)(U other) pure const nothrow if (isConvertible!U)
190 {
191 Quaternion conv = other;
192 return opEquals(conv);
193 }
194
195 /// Convert to a 3x3 rotation matrix.
196 /// TODO: check out why we can't do is(Unqual!U == mat3!T)
197 @nogc U opCast(U)() pure const nothrow if (is(typeof(U._isMatrix))
198 && is(U._T : _T)
199 && (U._R == 3) && (U._C == 3))
200 {
201 // do not assume that quaternion is normalized
202 T norm = x*x + y*y + z*z + w*w;
203 T s = (norm > 0) ? 2 / norm : 0;
204
205 T xx = x * x * s, xy = x * y * s, xz = x * z * s, xw = x * w * s,
206 yy = y * y * s, yz = y * z * s, yw = y * w * s,
207 zz = z * z * s, zw = z * w * s;
208 return mat3!(U._T)
209 (
210 1 - (yy + zz) , (xy - zw) , (xz + yw) ,
211 (xy + zw) , 1 - (xx + zz) , (yz - xw) ,
212 (xz - yw) , (yz + xw) , 1 - (xx + yy)
213 );
214 }
215
216 /// Converts a to a 4x4 rotation matrix.
217 /// Bugs: check why we can't do is(Unqual!U == mat4!T)
218 @nogc U opCast(U)() pure const nothrow if (is(typeof(U._isMatrix))
219 && is(U._T : _T)
220 && (U._R == 4) && (U._C == 4))
221 {
222 auto m3 = cast(mat3!(U._T))(this);
223 return cast(U)(m3);
224 }
225
226 // Workaround Vector not being constructable through CTFE
227 @nogc static Quaternion identity() pure nothrow @property
228 {
229 Quaternion q;
230 q.x = q.y = q.z = 0;
231 q.w = 1;
232 return q;
233 }
234 }
235
236 private
237 {
238 alias T _T;
239 enum _isQuaternion = true;
240
241 template isAssignable(T)
242 {
243 enum bool isAssignable =
244 is(typeof(
245 {
246 T x;
247 Quaternion v = x;
248 }()));
249 }
250
251 // define types that can be converted to Quaternion, but are not Quaternion
252 template isConvertible(T)
253 {
254 enum bool isConvertible = (!is(T : Quaternion)) && isAssignable!T;
255 }
256 }
257 }
258
259 alias Quaternion!float quatf;
260 alias Quaternion!double quatd;
261
262 /// Linear interpolation, for quaternions.
263 @nogc Quaternion!T lerp(T)(Quaternion!T a, Quaternion!T b, float t) pure nothrow
264 {
265 Quaternion!T res = void;
266 res.v = funcs.lerp(a.v, b.v, t);
267 return res;
268 }
269
270
271 /// Returns: Nlerp of quaternions.
272 /// See_also: $(WEB keithmaggio.wordpress.com/2011/02/15/math-magician-lerp-slerp-and-nlerp/, Math Magician – Lerp, Slerp, and Nlerp)
273 @nogc Quaternion!T Nlerp(T)(Quaternion!T a, Quaternion!T b, float t) pure nothrow
274 {
275 assert(t >= 0 && t <= 1); // else probably doesn't make sense
276 Quaternion!T res = void;
277 res.v = funcs.lerp(a.v, b.v, t);
278 res.v.normalize();
279 return res;
280 }
281
282 /// Returns: Slerp of quaternions. Slerp is more expensive than Nlerp.
283 /// See_also: "Understanding Slerp, Then Not Using It"
284 @nogc Quaternion!T slerp(T)(Quaternion!T a, Quaternion!T b, T t) pure nothrow
285 {
286 assert(t >= 0 && t <= 1); // else probably doesn't make sense
287
288 Quaternion!T res = void;
289
290 T dotProduct = dot(a.v, b.v);
291
292 // spherical lerp always has 2 potential paths
293 // here we always take the shortest
294 if (dotProduct < 0)
295 {
296 b.v *= -1;
297 dotProduct = dot(a.v, b.v);
298 }
299
300 immutable T threshold = 10 * T.epsilon; // idMath uses 1e-6f for 32-bits float precision
301 if ((1 - dotProduct) > threshold) // if small difference, use lerp
302 return lerp(a, b, t);
303
304 T theta_0 = funcs.safeAcos(dotProduct); // angle between this and other
305 T theta = theta_0 * t; // angle between this and result
306
307 vec3!T v2 = dot(b.v, a.v * dotProduct);
308 v2.normalize();
309
310 res.v = dot(b.v, a.v * dotProduct);
311 res.v.normalize();
312
313 res.v = a.v * cos(theta) + res.v * sin(theta);
314 return res;
315 }
316
317 unittest
318 {
319 quatf a = quatf.fromAxis(vec3f(1, 0, 0), 1);
320 quatf b = quatf.fromAxis(vec3f(0, 1, 0), 0);
321 a = a * b;
322
323 quatf c = lerp(a, b, 0.5f);
324 quatf d = Nlerp(a, b, 0.1f);
325 quatf e = slerp(a, b, 0.0f);
326 quatd f = quatd(1.0, 4, 5.0, 6.0);
327 quatf g = quatf.fromEulerAngles(-0.1f, 1.2f, -0.3f);
328 vec3f ga = g.toEulerAngles();
329 }