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