gfm.math.quaternion source code

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