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 if (isAssignable!U)
29         {
30             opAssign!U(x);
31         }
32 
33         /// Constructs a Quaternion from axis + angle.
34         static Quaternion fromAxis(Vector!(T, 3u) axis, T angle) pure nothrow
35         {
36             Quaternion q = void;
37             axis.normalize();
38             T cos_a = cos(angle / 2);
39             T sin_a = sin(angle / 2);
40             q.x = sin_a * axis.x;
41             q.y = sin_a * axis.y;
42             q.z = sin_a * axis.z;
43             q.w = cos_a;
44             return q; // should be normalized
45         }
46 
47         /// Assign from another Quaternion.
48         ref Quaternion opAssign(U)(U u) pure nothrow if (is(typeof(U._isQuaternion)) && is(U._T : T))
49         {
50             v = u.v;
51             return this;
52         }
53 
54         /// Assign from a vector of 4 elements.
55         ref Quaternion opAssign(U)(U u) pure nothrow if (is(U : Vector!(T, 4u)))
56         {
57             v = u;
58             return this;
59         }
60 
61         /// Converts to a pretty string.
62         string toString() const nothrow
63         {
64             try
65                 return format("%s", v);
66             catch (Exception e) 
67                 assert(false); // should not happen since format is right
68         }
69 
70         /// Normalizes a quaternion.
71         void normalize() pure nothrow
72         {
73             v.normalize();
74         }
75 
76         /// Returns: Normalized quaternion.
77         Quaternion normalized() pure const nothrow
78         {
79             Quaternion res = void;
80             res.v = v.normalized();
81             return res;
82         }
83 
84         /// Inverse a quaternion.
85         void inverse() pure nothrow
86         {
87             x = -x;
88             y = -y;
89             z = -z;
90         }
91 
92         /// Returns: Inversed quaternion.
93         Quaternion inversed() pure const nothrow
94         {
95             Quaternion res = void;
96             res.v = v;
97             res.inverse();
98             return res;
99         }
100 
101         ref Quaternion opOpAssign(string op, U)(U q) pure nothrow
102             if (is(U : Quaternion) && (op == "*"))
103         {
104             T nx = w * q.x + x * q.w + y * q.z - z * q.y,
105               ny = w * q.y + y * q.w + z * q.x - x * q.z,
106               nz = w * q.z + z * q.w + x * q.y - y * q.x,
107               nw = w * q.w - x * q.x - y * q.y - z * q.z;
108             x = nx;
109             y = ny;
110             z = nz;
111             w = nw;
112             return this;
113         }
114 
115         ref Quaternion opOpAssign(string op, U)(U operand) pure nothrow if (isConvertible!U)
116         {
117             Quaternion conv = operand;
118             return opOpAssign!op(conv);
119         }
120 
121         Quaternion opBinary(string op, U)(U operand) pure const nothrow
122             if (is(U: Quaternion) || (isConvertible!U))
123         {
124             Quaternion temp = this;
125             return temp.opOpAssign!op(operand);
126         }
127 
128         /// Compare two Quaternions.
129         bool opEquals(U)(U other) pure const if (is(U : Quaternion))
130         {
131             return v == other.v;
132         }
133 
134         /// Compare Quaternion and other types.
135         bool opEquals(U)(U other) pure const nothrow if (isConvertible!U)
136         {
137             Quaternion conv = other;
138             return opEquals(conv);
139         }
140 
141         /// Convert to a 3x3 rotation matrix.
142         /// TODO: check out why we can't do is(Unqual!U == mat3!T)
143         U opCast(U)() pure const nothrow if (is(typeof(U._isMatrix))
144                                           && is(U._T : _T)
145                                           && (U._R == 3) && (U._C == 3))
146         {
147             // do not assume that quaternion is normalized
148             T norm = x*x + y*y + z*z + w*w;
149             T s = (norm > 0) ? 2 / norm : 0;
150 
151             T xx = x * x * s, xy = x * y * s, xz = x * z * s, xw = x * w * s,
152                               yy = y * y * s, yz = y * z * s, yw = y * w * s,
153                                               zz = z * z * s, zw = z * w * s;
154             return mat3!(U._T)
155             (
156                 1 - (yy + zz)   ,   (xy - zw)    ,   (xz + yw)    ,
157                   (xy + zw)     , 1 - (xx + zz)  ,   (yz - xw)    ,
158                   (xz - yw)     ,   (yz + xw)    , 1 - (xx + yy)
159             );
160         }
161 
162         /// Converts a to a 4x4 rotation matrix.
163         /// Bugs: check why we can't do is(Unqual!U == mat4!T)
164         U opCast(U)() pure const nothrow if (is(typeof(U._isMatrix))
165                                           && is(U._T : _T)
166                                           && (U._R == 4) && (U._C == 4))
167         {
168             auto m3 = cast(mat3!(U._T))(this);
169             return cast(U)(m3);
170         }
171 
172         // Workaround Vector not being constructable through CTFE
173         static Quaternion IDENTITY() pure nothrow @property
174         {
175             Quaternion q;
176             q.x = q.y = q.z = 0;
177             q.w = 1;
178             return q;
179         }
180     }
181 
182     private
183     {
184         alias T _T;
185         enum _isQuaternion = true;
186 
187         template isAssignable(T)
188         {
189             enum bool isAssignable =
190                 is(typeof(
191                 {
192                     T x;
193                     Quaternion v = x;
194                 }()));
195         }
196 
197         // define types that can be converted to Quaternion, but are not Quaternion
198         template isConvertible(T)
199         {
200             enum bool isConvertible = (!is(T : Quaternion)) && isAssignable!T;
201         }
202     }
203 }
204 
205 alias Quaternion!float quaternionf;
206 alias Quaternion!double quaterniond;
207 
208 /// Linear interpolation, for quaternions.
209 Quaternion!T lerp(T)(Quaternion!T a, Quaternion!T b, float t) pure nothrow
210 {
211     Quaternion!T res = void;
212     res.v = funcs.lerp(a.v, b.v, t);
213     return res;
214 }
215 
216 
217 /// Returns: Nlerp of quaternions.
218 /// See_also: $(WEB keithmaggio.wordpress.com/2011/02/15/math-magician-lerp-slerp-and-nlerp/, Math Magician – Lerp, Slerp, and Nlerp)
219 Quaternion!T Nlerp(T)(Quaternion!T a, Quaternion!T b, float t) pure nothrow
220 {
221     assert(t >= 0 && t <= 1); // else probably doesn't make sense 
222     Quaternion!T res = void;
223     res.v = funcs.lerp(a.v, b.v, t);
224     res.v.normalize();
225     return res;
226 }
227 
228 /// Returns: Slerp of quaternions. Slerp is more expensive than Nlerp.
229 /// See_also: "Understanding Slerp, Then Not Using It"
230 Quaternion!T slerp(T)(Quaternion!T a, Quaternion!T b, T t) pure nothrow
231 {
232     assert(t >= 0 && t <= 1); // else probably doesn't make sense 
233 
234     Quaternion!T res = void;
235 
236     T dotProduct = dot(a.v, b.v);
237 
238     // spherical lerp always has 2 potential paths
239     // here we always take the shortest
240     if (dotProduct < 0)
241     {
242         b.v *= -1;
243         dotProduct = dot(a.v, b.v);
244     }
245 
246     immutable T threshold = 10 * T.epsilon; // idMath uses 1e-6f for 32-bits float precision
247     if ((1 - dotProduct) > threshold) // if small difference, use lerp
248         return lerp(a, b, t);
249 
250     T theta_0 = funcs.safeAcos(dotProduct); // angle between this and other
251     T theta = theta_0 * t; // angle between this and result
252 
253     vec3!T v2 = dot(b.v, a.v * dotProduct);
254     v2.normalize();
255 
256     res.v = dot(b.v, a.v * dotProduct);
257     res.v.normalize();
258 
259     res.v = a.v * cos(theta) + res.v * sin(theta);
260     return res;
261 }
262 
263 unittest
264 {
265     quaternionf a = quaternionf.fromAxis(vec3f(1, 0, 0), 1);
266     quaternionf b = quaternionf.fromAxis(vec3f(0, 1, 0), 0);
267     a = a * b;
268 
269     quaternionf c = lerp(a, b, 0.5f);
270     quaternionf d = Nlerp(a, b, 0.1f);
271     quaternionf e = slerp(a, b, 0.0f);
272 }