gfm.math.quaternion source code

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 (isQuaternionInstantiation!U && 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 (isMatrixInstantiation!U
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 (isMatrixInstantiation!U
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 
240         template isAssignable(T)
241         {
242             enum bool isAssignable =
243                 is(typeof(
244                 {
245                     T x;
246                     Quaternion v = x;
247                 }()));
248         }
249 
250         // define types that can be converted to Quaternion, but are not Quaternion
251         template isConvertible(T)
252         {
253             enum bool isConvertible = (!is(T : Quaternion)) && isAssignable!T;
254         }
255     }
256 }
257 
258 template isQuaternionInstantiation(U)
259 {
260     private static void isQuaternion(T)(Quaternion!T x)
261     {
262     }
263 
264     enum bool isQuaternionInstantiation = is(typeof(isQuaternion(U.init)));
265 }
266 
267 alias Quaternion!float quatf;
268 alias Quaternion!double quatd;
269 
270 /// Linear interpolation, for quaternions.
271 @nogc Quaternion!T lerp(T)(Quaternion!T a, Quaternion!T b, float t) pure nothrow
272 {
273     Quaternion!T res = void;
274     res.v = funcs.lerp(a.v, b.v, t);
275     return res;
276 }
277 
278 
279 /// Returns: Nlerp of quaternions.
280 /// See_also: $(WEB keithmaggio.wordpress.com/2011/02/15/math-magician-lerp-slerp-and-nlerp/, Math Magician – Lerp, Slerp, and Nlerp)
281 @nogc Quaternion!T Nlerp(T)(Quaternion!T a, Quaternion!T b, float t) pure nothrow
282 {
283     assert(t >= 0 && t <= 1); // else probably doesn't make sense
284     Quaternion!T res = void;
285     res.v = funcs.lerp(a.v, b.v, t);
286     res.v.normalize();
287     return res;
288 }
289 
290 /// Returns: Slerp of quaternions. Slerp is more expensive than Nlerp.
291 /// See_also: "Understanding Slerp, Then Not Using It"
292 @nogc Quaternion!T slerp(T)(Quaternion!T a, Quaternion!T b, T t) pure nothrow
293 {
294     assert(t >= 0 && t <= 1); // else probably doesn't make sense
295 
296     Quaternion!T res = void;
297 
298     T dotProduct = dot(a.v, b.v);
299 
300     // spherical lerp always has 2 potential paths
301     // here we always take the shortest
302     if (dotProduct < 0)
303     {
304         b.v *= -1;
305         dotProduct = dot(a.v, b.v);
306     }
307 
308     immutable T threshold = 10 * T.epsilon; // idMath uses 1e-6f for 32-bits float precision
309     if ((1 - dotProduct) > threshold) // if small difference, use lerp
310         return lerp(a, b, t);
311 
312     T theta_0 = funcs.safeAcos(dotProduct); // angle between this and other
313     T theta = theta_0 * t; // angle between this and result
314 
315     vec3!T v2 = dot(b.v, a.v * dotProduct);
316     v2.normalize();
317 
318     res.v = dot(b.v, a.v * dotProduct);
319     res.v.normalize();
320 
321     res.v = a.v * cos(theta) + res.v * sin(theta);
322     return res;
323 }
324 
325 unittest
326 {
327     quatf a = quatf.fromAxis(vec3f(1, 0, 0), 1);
328     quatf b = quatf.fromAxis(vec3f(0, 1, 0), 0);
329     a = a * b;
330 
331     quatf c = lerp(a, b, 0.5f);
332     quatf d = Nlerp(a, b, 0.1f);
333     quatf e = slerp(a, b, 0.0f);
334     quatd f = quatd(1.0, 4, 5.0, 6.0);
335     quatf g = quatf.fromEulerAngles(-0.1f, 1.2f, -0.3f);
336     vec3f ga = g.toEulerAngles();
337 }