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