armos.math.quaternion source code

1 module armos.math.quaternion;
2 import armos.math;
3 import std.math;
4 /++
5 クオータニオンを表すstructです．
6 +/
7 struct Quaternion(T)if(__traits(isArithmetic, T)){
8     alias Quaternion!(T) Q;
9     alias V4 = armos.math.Vector!(T, 4);
10     alias V3 = armos.math.Vector!(T, 3);
11 
12     /// vec[0];x
13     /// vec[1];y
14     /// vec[2];z
15     /// vec[3];w
16     V4 vec = V4();
17 
18     /++
19     +/
20     this(in T x, in T y, in T z, in T w){
21         vec[0] = x;
22         vec[1] = y;
23         vec[2] = z;
24         vec[3] = w;
25     }
26     unittest{
27         alias N = double;
28         alias Q = Quaternion!N;
29         assert(__traits(compiles, { Q q = Q(N(1), N(2), N(3), N(4)); }));
30     }
31     unittest{
32         assert(__traits(compiles, { auto q = Quaternion!(int)(0, 0, 0, 1); }));
33     }
34 
35 
36     /++
37     +/
38     this(in T s, in V3 v){
39         vec[0] = v[0];
40         vec[1] = v[1];
41         vec[2] = v[2];
42         vec[3] = s;
43     }
44     unittest{
45         alias T = double;
46         alias Q = Quaternion!T;
47         alias V3 = Vector!(T, 3);
48         assert(__traits(compiles, {
49                     Q q = Q(T(1), V3(2, 3, 4));
50                     }));
51     }
52 
53     /++
54     +/
55     this(in V3 v){
56         this(T(1), v);
57     }
58     unittest{
59         alias T = double;
60         alias Q = Quaternion!T;
61         alias V3 = Vector!(T, 3);
62         assert(__traits(compiles, {
63                     Q q = Q(V3(2, 3, 4));
64                     }));
65     }
66 
67     /++
68     +/
69     this(in V4 v){
70         vec[0] = v[0];
71         vec[1] = v[1];
72         vec[2] = v[2];
73         vec[3] = v[3];
74     }
75     unittest{
76         alias T = double;
77         alias Q = Quaternion!T;
78         alias V4 = Vector!(T, 4);
79         assert(__traits(compiles, {
80                     Q q = Q(V4(1, 2, 3, 4));
81                     }));
82     }
83 
84     /++
85     +/
86     T opIndex(in int index)const{
87         return vec[index];
88     }
89 
90     /++
91     +/
92     ref  T opIndex(in int index){
93         return vec[index];
94     }
95     unittest{
96         Quaternion!(double) q = Quaternion!(double)(1, 2, 3, 4);
97         assert(q[0]==1);
98         assert(q[1]==2);
99         assert(q[2]==3);
100         assert(q[3]==4);
101     }
102 
103     /++
104     +/
105     static Q zero(){
106         return Q(T( 0 ), T( 0 ), T( 0 ), T( 0 ));
107     }
108     unittest{
109         auto e = Quaternion!(double).zero;
110         assert(e[0]==0);
111         assert(e[1]==0);
112         assert(e[2]==0);
113         assert(e[3]==0);
114     }
115 
116     /++
117     +/
118     static Q unit(){
119         return Q(T( 0 ), T( 0 ), T( 0 ), T( 1 ));
120     }
121     unittest{
122         auto e = Quaternion!(double).unit;
123         assert(e[0]==0);
124         assert(e[1]==0);
125         assert(e[2]==0);
126         assert(e[3]==1);
127     }
128 
129     /++
130     +/
131     Q opMul(in Q r_quat)const{
132         immutable T s_l  = this[3];
133         immutable v_l = V3(this[0], this[1], this[2]);
134 
135         immutable T s_r  = r_quat[3];
136         immutable v_r = V3(r_quat[0], r_quat[1], r_quat[2],);
137 
138         immutable return_v = s_l*v_r + s_r*v_l + v_l.vectorProduct(v_r) ;
139         immutable return_s = ( s_l*s_r ) - ( v_l.dotProduct(v_r) );
140         return Q(return_v[0], return_v[1], return_v[2], return_s);
141     }
142     unittest{
143         Quaternion!(double) q1      = Quaternion!(double)(0, 0, 0, 1);
144         Quaternion!(double) q2      = Quaternion!(double)(0, 0, 0, 1);
145         Quaternion!(double) qAnswer = Quaternion!(double)(0, 0, 0, 1);
146         assert(q1*q2 == qAnswer);
147     }
148     unittest{
149         auto va = Vector!(double, 3)(0.5, 0.1, 0.4);
150         auto v = va.normalized;
151 
152         Quaternion!(double) q1      = Quaternion!(double)(v[0]*sin(0.1), v[1]*sin(0.1), v[2]*sin(0.1), cos(0.1));
153         Quaternion!(double) q2      = Quaternion!(double)(v[0]*sin(0.2), v[1]*sin(0.2), v[2]*sin(0.2), cos(0.2));
154         Quaternion!(double) qResult = q1 * q2;
155         Quaternion!(double) qAnswer = Quaternion!(double)(v[0]*sin(0.3), v[1]*sin(0.3), v[2]*sin(0.3), cos(0.3));
156         foreach (int i, value; qResult.vec.elements) {
157             assert( approxEqual(value, qAnswer[i]) );
158         }
159     }
160 
161     /++
162     +/
163     Q opNeg()const{
164         return Q(-this[0], -this[1], -this[2], -this[3]);
165     }
166     unittest{
167         Quaternion!(double) q = Quaternion!(double)(1, 1, 1, 1);
168         Quaternion!(double) a = Quaternion!(double)(-1, -1, -1, -1);
169         assert(-q == a);
170     }
171 
172     /++
173     +/
174     Q opAdd(in Q q)const{
175         auto return_quat = Q();
176         return_quat.vec = vec + q.vec;
177         return return_quat;
178     }
179     unittest{
180         auto q1 = Quaternion!(double)(1, 2, 3, 4);
181         auto q2 = Quaternion!(double)(2, 3, 4, 5);
182         auto qA = Quaternion!(double)(3, 5, 7, 9);
183         assert(q1+q2 == qA);
184     }
185 
186     /++
187     +/
188     Q opMul(in T r)const{
189         auto return_quat = Q();
190         return_quat.vec = vec * r;
191         return return_quat;
192     }
193     unittest{
194         auto q = Quaternion!(double)(1, 2, 3, 4);
195         auto qA = Quaternion!(double)(3, 6, 9, 12);
196         assert(q*3 == qA);
197     }
198 
199     /++
200     +/
201     Q opDiv(in T r)const {
202         auto return_quat = Q();
203         return_quat.vec = vec / r;
204         return return_quat;
205     }
206     unittest{
207         auto q = Quaternion!(double)(3, 6, 9, 12);
208         auto qA = Quaternion!(double)(1, 2, 3, 4);
209         assert(q/3 == qA);
210     }
211 
212     ///
213     CastedType opCast(CastedType: Quaternion!T, T)()const{
214         import std.conv:to;
215         return CastedType(this.vec.to!(typeof(CastedType.vec)));
216     }
217 
218     unittest{ 
219         import std.conv:to;
220         Quaternion!int iq = Quaternion!double.zero.to!(Quaternion!int); 
221         import std.conv:to;
222         Quaternion!double dq = Quaternion!int.zero.to!(Quaternion!double); 
223     }
224 
225     ///
226     CastedType opCast(CastedType: Matrix!(E, 4, 4), E)()const{
227         import std.conv:to;
228         return matrix44!E;
229     }
230 
231     unittest{ 
232         import std.conv:to;
233         auto q = Quaternion!double.unit.to!(Matrix!(float, 4, 4));
234         auto m = Matrix!(float, 4, 4).identity;
235         assert(q == m);
236     }
237 
238     ///
239     CastedType opCast(CastedType: Matrix!(E, 3, 3), E)()const{
240         import std.conv:to;
241         return matrix33!E;
242     }
243 
244     unittest{ 
245         import std.conv:to;
246         auto q = Quaternion!double.unit.to!(Matrix!(float, 3, 3));
247         auto m = Matrix!(float, 3, 3).identity;
248         assert(q == m);
249     }
250     
251     /++
252         Quaternionのノルムを返します．
253     +/
254     T norm()()const if(__traits(isFloating, T)){
255         return sqrt(this[0]^^2.0 + this[1]^^2.0 + this[2]^^2.0 + this[3]^^2.0);
256     }
257 
258     unittest{
259         auto q = Quaternion!(double)(1, 2, 3, 4);
260         assert(q.norm == sqrt(30.0));
261     }
262 
263     /++
264     +/
265     Q normalized()const {
266         return Q(vec.normalized);
267     }
268     unittest{
269         // auto q = Quaternion!(double)(1, 2, 3, 4);
270         // assert(q.norm == sqrt(30.0));
271     }
272 
273     /++
274         Quaternionの共役Quaternionを返します．
275     +/
276     Q conjugate()const{
277         return Q(-this[0], -this[1], -this[2], this[3]);
278     }
279     unittest{
280         auto q  = Quaternion!(double)(1, 2, 3, 4);
281         auto qA = Quaternion!(double)(-1, -2, -3, 4);
282         assert(q.conjugate == qA);
283     }
284 
285     /++
286 
287     +/
288     Q inverse()()const if(__traits(isFloating, T)){
289         return conjugate/(this[0]^^T(2.0) + this[1]^^T(2.0) + this[2]^^T(2.0) + this[3]^^T(2.0));
290     }
291     unittest{
292         auto q  = Quaternion!(double)(0, 1, 0, 1);
293         auto qR = q.inverse;
294         auto qA = Quaternion!(double)(0, -0.5, 0, 0.5);
295 
296         foreach (int i, value; qR.vec.elements) {
297             assert( approxEqual(value, qA[i]) );
298         }
299     }
300 
301     /++
302         自身の回転行列(4x4)を返します．
303     +/
304     armos.math.Matrix!(E, 4, 4) matrix44(E = T)()const if(__traits(isFloating, E)){
305         return armos.math.Matrix!(E, 4, 4)(
306                 [this[3]^^2.0+this[0]^^2.0-this[1]^^2.0-this[2]^^2.0, 2.0*(this[0]*this[1]-this[3]*this[2]),               2.0*(this[0]*this[2]+this[3]*this[1]),               0],
307                 [2.0*(this[0]*this[1]+this[3]*this[2]),               this[3]^^2.0-this[0]^^2.0+this[1]^^2.0-this[2]^^2.0, 2.0*(this[1]*this[2]-this[3]*this[0]),               0],
308                 [2.0*(this[0]*this[2]-this[3]*this[1]),               2.0*(this[1]*this[2]+this[3]*this[0]),               this[3]^^2.0-this[0]^^2.0-this[1]^^2.0+this[2]^^2.0, 0],
309                 [0,                                                   0,                                                   0,                                                   1]
310                 );
311     }
312     unittest{
313     }
314 
315     /++
316         自身の回転行列(3x3)を返します．
317     +/
318     armos.math.Matrix!(E, 3, 3) matrix33(E = T)()const if(__traits(isFloating, E)){
319         return armos.math.Matrix!(E, 3, 3)(
320                 [this[3]^^2.0+this[0]^^2.0-this[1]^^2.0-this[2]^^2.0, 2.0*(this[0]*this[1]-this[3]*this[2]),               2.0*(this[0]*this[2]+this[3]*this[1])              ],
321                 [2.0*(this[0]*this[1]+this[3]*this[2]),               this[3]^^2.0-this[0]^^2.0+this[1]^^2.0-this[2]^^2.0, 2.0*(this[1]*this[2]-this[3]*this[0])              ],
322                 [2.0*(this[0]*this[2]-this[3]*this[1]),               2.0*(this[1]*this[2]+this[3]*this[0]),               this[3]^^2.0-this[0]^^2.0-this[1]^^2.0+this[2]^^2.0]
323                 );
324     }
325 
326     /++
327         指定したベクトルを自身で回転させたベクトルを返します．
328     +/
329     V3 rotatedVector()(in V3 vec)const if(__traits(isFloating, T)){
330         if( norm^^2.0 < T.epsilon){
331             return vec;
332         }else{
333             auto temp_quat = Q(vec);
334             auto return_quat= this*temp_quat*this.inverse;
335             auto return_vector = V3(return_quat[0], return_quat[1], return_quat[2]);
336             return return_vector;
337         }
338     }
339     unittest{
340         auto v = armos.math.Vector3d(1, 0, 0);
341         double ang = PI*0.5*0.5;
342         auto q  = Quaternion!(double)(0*sin(ang), 0*sin(ang), 1*sin(ang), cos(ang));
343         auto vR = q.rotatedVector(v);
344         auto vA = armos.math.Vector3d(0, 1, 0);
345 
346         foreach (int i, value; vR.elements) {
347             assert( approxEqual(value, vA[i]) );
348         }
349     }
350 
351     /++
352         指定したベクトルを自身の逆方向に回転させたベクトルを返します．
353     +/
354     V3 rotatedVectorInversely()(in V3 vec)const if(__traits(isFloating, T)){
355         if( norm^^2.0 < T.epsilon){
356             return vec;
357         }else{
358             auto temp_quat = Q(vec);
359             auto return_quat= this.inverse*temp_quat*this;
360             auto return_vector = V3(return_quat[0], return_quat[1], return_quat[2]);
361             return return_vector;
362         }
363     }
364     unittest{
365         auto v = armos.math.Vector3d(1, 0, 0);
366         double ang = PI*0.5*0.5;
367         auto q  = Quaternion!(double)(0*sin(ang), 0*sin(ang), 1*sin(ang), cos(ang));
368         auto vR = q.rotatedVectorInversely(v);
369         auto vA = armos.math.Vector3d(0, -1, 0);
370 
371         foreach (int i, value; vR.elements) {
372             assert( approxEqual(value, vA[i]) );
373         }
374     }
375 
376     /++
377         指定した軸で自身を回転させます．
378         Params:
379         ang = 回転角
380         axis = 回転軸
381     +/
382     static Q angleAxis()(T ang, V3 axis) if(__traits(isFloating, T)){
383         immutable halfAngle = ang*T(0.5);
384         return Q(axis[0]*sin(halfAngle), axis[1]*sin(halfAngle), axis[2]*sin(halfAngle), cos(halfAngle));
385     }
386     unittest{
387         auto v = armos.math.Vector3d(1, 0, 0);
388         auto q = Quaternion!(double).angleAxis(PI*0.5, armos.math.Vector3d(0, 0, 1));
389         auto vR = q.rotatedVector(v);
390         auto vA = armos.math.Vector3d(0, 1, 0);
391 
392         foreach (int i, value; vR.elements) {
393             assert( approxEqual(value, vA[i]) );
394         }
395     }
396 
397     Q productAngle()(T gain) if(__traits(isFloating, T)){
398         return slerp(Q.zero, this, gain);
399     }
400 
401 }
402 
403 /// 回転の差分のQuaternionを返します．
404 Q rotationDifference(Q)(in Q from,  in Q to){
405     return from.inverse * to;
406 }
407 unittest{
408     auto qBegin= Quaternion!(double).angleAxis(PI*0.2, armos.math.Vector3d(0, 0, 1));
409     auto qEnd = Quaternion!(double).angleAxis(PI*0.5, armos.math.Vector3d(0, 0, 1));
410     
411     auto qAns = Quaternion!(double).angleAxis(PI*0.3, armos.math.Vector3d(0, 0, 1));
412     auto qResult = rotationDifference(qBegin, qEnd);
413     
414     assert(approxEqual(qResult[0], qAns[0]));
415     assert(approxEqual(qResult[1], qAns[1]));
416     assert(approxEqual(qResult[2], qAns[2]));
417     assert(approxEqual(qResult[3], qAns[3]));
418 }
419 
420 /++
421 球面線形補間(Sphercal Linear Interpolation)を行います．
422 Params:
423 from = tが0の時のQuaternion
424 to = tが1の時のQuaternion
425 t = 
426 +/
427 Q slerp(Q, T)(in Q from, in Q to,  T t)if(__traits(isFloating, typeof(Q.vec[0]))){
428     T omega, cos_omega, sin_omega, scale_from, scale_to;
429 
430     Q quatTo = to;
431     cos_omega = from.vec.dotProduct(to.vec);
432 
433     if (cos_omega < T( 0.0 )) {
434         cos_omega = -cos_omega;
435         quatTo = -to;
436     }
437 
438     if( (T( 1.0 ) - cos_omega) > T.epsilon ){
439         omega = acos(cos_omega);
440         sin_omega = sin(omega);
441         scale_from = sin(( T( 1.0 ) - t ) * omega) / sin_omega;
442         scale_to = sin(t * omega) / sin_omega;
443     }else{
444         scale_from = T( 1.0 ) - t;
445         scale_to = t;
446     }
447 
448     return ( from * scale_from ) + ( quatTo * scale_to  );
449 }
450 unittest{
451     auto v = armos.math.Vector3d(1, 0, 0);
452     auto qBegin= Quaternion!(double).angleAxis(0, armos.math.Vector3d(0, 0, 1));
453     auto qEnd = Quaternion!(double).angleAxis(PI*0.5, armos.math.Vector3d(0, 0, 1));
454     auto qSlerped = qBegin.slerp(qEnd, 2.0);
455     auto vR = qSlerped.rotatedVector(v);
456     auto vA = armos.math.Vector3d(-1, 0, 0);
457 
458     foreach (int i, value; vR.elements) {
459         assert( approxEqual(value, vA[i]) );
460     }
461 
462 }
463 
464 alias Quaternion!(float) Quaternionf;
465 alias Quaternion!(double) Quaterniond;