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     CastedType opCast(CastedType)()const{
213         import std.conv:to;
214         return CastedType(this.vec.to!(typeof(CastedType.vec)));
215     }
216 
217     unittest{ 
218         import std.conv:to;
219         Quaternion!int iq = Quaternion!double.zero.to!(Quaternion!int); 
220         import std.conv:to;
221         Quaternion!double dq = Quaternion!int.zero.to!(Quaternion!double); 
222     }
223 
224     /++
225         Quaternionのノルムを返します．
226     +/
227     T norm()()const if(__traits(isFloating, T)){
228         return sqrt(this[0]^^2.0 + this[1]^^2.0 + this[2]^^2.0 + this[3]^^2.0);
229     }
230 
231     unittest{
232         auto q = Quaternion!(double)(1, 2, 3, 4);
233         assert(q.norm == sqrt(30.0));
234     }
235 
236     /++
237     +/
238     Q normalized()const {
239         return Q(vec.normalized);
240     }
241     unittest{
242         // auto q = Quaternion!(double)(1, 2, 3, 4);
243         // assert(q.norm == sqrt(30.0));
244     }
245 
246     /++
247         Quaternionの共役Quaternionを返します．
248     +/
249     Q conjugate()const{
250         return Q(-this[0], -this[1], -this[2], this[3]);
251     }
252     unittest{
253         auto q  = Quaternion!(double)(1, 2, 3, 4);
254         auto qA = Quaternion!(double)(-1, -2, -3, 4);
255         assert(q.conjugate == qA);
256     }
257 
258     /++
259 
260     +/
261     Q inverse()()const if(__traits(isFloating, T)){
262         return conjugate/(this[0]^^T(2.0) + this[1]^^T(2.0) + this[2]^^T(2.0) + this[3]^^T(2.0));
263     }
264     unittest{
265         auto q  = Quaternion!(double)(0, 1, 0, 1);
266         auto qR = q.inverse;
267         auto qA = Quaternion!(double)(0, -0.5, 0, 0.5);
268 
269         foreach (int i, value; qR.vec.elements) {
270             assert( approxEqual(value, qA[i]) );
271         }
272     }
273 
274     /++
275         自身の回転行列(4x4)を返します．
276     +/
277     armos.math.Matrix!(T, 4, 4) matrix44()()const if(__traits(isFloating, T)){
278         return armos.math.Matrix!(T, 4, 4)(
279                 [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],
280                 [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],
281                 [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],
282                 [0,                                                   0,                                                   0,                                                   1]
283                 );
284     }
285     unittest{
286     }
287 
288     /++
289         自身の回転行列(3x3)を返します．
290     +/
291     armos.math.Matrix!(T, 3, 3) matrix33()()const if(__traits(isFloating, T)){
292         return armos.math.Matrix!(T, 3, 3)(
293                 [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])              ],
294                 [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])              ],
295                 [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]
296                 );
297     }
298 
299     /++
300         指定したベクトルを自身で回転させたベクトルを返します．
301     +/
302     V3 rotatedVector()(in V3 vec)const if(__traits(isFloating, T)){
303         if( norm^^2.0 < T.epsilon){
304             return vec;
305         }else{
306             auto temp_quat = Q(vec);
307             auto return_quat= this*temp_quat*this.inverse;
308             auto return_vector = V3(return_quat[0], return_quat[1], return_quat[2]);
309             return return_vector;
310         }
311     }
312     unittest{
313         auto v = armos.math.Vector3d(1, 0, 0);
314         double ang = PI*0.5*0.5;
315         auto q  = Quaternion!(double)(0*sin(ang), 0*sin(ang), 1*sin(ang), cos(ang));
316         auto vR = q.rotatedVector(v);
317         auto vA = armos.math.Vector3d(0, 1, 0);
318 
319         foreach (int i, value; vR.elements) {
320             assert( approxEqual(value, vA[i]) );
321         }
322     }
323 
324     /++
325         指定したベクトルを自身の逆方向に回転させたベクトルを返します．
326     +/
327     V3 rotatedVectorInversely()(in V3 vec)const if(__traits(isFloating, T)){
328         if( norm^^2.0 < T.epsilon){
329             return vec;
330         }else{
331             auto temp_quat = Q(vec);
332             auto return_quat= this.inverse*temp_quat*this;
333             auto return_vector = V3(return_quat[0], return_quat[1], return_quat[2]);
334             return return_vector;
335         }
336     }
337     unittest{
338         auto v = armos.math.Vector3d(1, 0, 0);
339         double ang = PI*0.5*0.5;
340         auto q  = Quaternion!(double)(0*sin(ang), 0*sin(ang), 1*sin(ang), cos(ang));
341         auto vR = q.rotatedVectorInversely(v);
342         auto vA = armos.math.Vector3d(0, -1, 0);
343 
344         foreach (int i, value; vR.elements) {
345             assert( approxEqual(value, vA[i]) );
346         }
347     }
348 
349     /++
350         指定した軸で自身を回転させます．
351         Params:
352         ang = 回転角
353         axis = 回転軸
354     +/
355     static Q angleAxis()(T ang, V3 axis) if(__traits(isFloating, T)){
356         immutable halfAngle = ang*T(0.5);
357         return Q(axis[0]*sin(halfAngle), axis[1]*sin(halfAngle), axis[2]*sin(halfAngle), cos(halfAngle));
358     }
359     unittest{
360         auto v = armos.math.Vector3d(1, 0, 0);
361         auto q = Quaternion!(double).angleAxis(PI*0.5, armos.math.Vector3d(0, 0, 1));
362         auto vR = q.rotatedVector(v);
363         auto vA = armos.math.Vector3d(0, 1, 0);
364 
365         foreach (int i, value; vR.elements) {
366             assert( approxEqual(value, vA[i]) );
367         }
368     }
369 
370     Q productAngle()(T gain) if(__traits(isFloating, T)){
371         return slerp(Q.zero, this, gain);
372     }
373 
374 }
375 
376 /// 回転の差分のQuaternionを返します．
377 Q rotationDifference(Q)(in Q from,  in Q to){
378     return from.inverse * to;
379 }
380 unittest{
381     auto qBegin= Quaternion!(double).angleAxis(PI*0.2, armos.math.Vector3d(0, 0, 1));
382     auto qEnd = Quaternion!(double).angleAxis(PI*0.5, armos.math.Vector3d(0, 0, 1));
383     
384     auto qAns = Quaternion!(double).angleAxis(PI*0.3, armos.math.Vector3d(0, 0, 1));
385     auto qResult = rotationDifference(qBegin, qEnd);
386     
387     assert(approxEqual(qResult[0], qAns[0]));
388     assert(approxEqual(qResult[1], qAns[1]));
389     assert(approxEqual(qResult[2], qAns[2]));
390     assert(approxEqual(qResult[3], qAns[3]));
391 }
392 
393 /++
394 球面線形補間(Sphercal Linear Interpolation)を行います．
395 Params:
396 from = tが0の時のQuaternion
397 to = tが1の時のQuaternion
398 t = 
399 +/
400 Q slerp(Q, T)(in Q from, in Q to,  T t)if(__traits(isFloating, typeof(Q.vec[0]))){
401     T omega, cos_omega, sin_omega, scale_from, scale_to;
402 
403     Q quatTo = to;
404     cos_omega = from.vec.dotProduct(to.vec);
405 
406     if (cos_omega < T( 0.0 )) {
407         cos_omega = -cos_omega;
408         quatTo = -to;
409     }
410 
411     if( (T( 1.0 ) - cos_omega) > T.epsilon ){
412         omega = acos(cos_omega);
413         sin_omega = sin(omega);
414         scale_from = sin(( T( 1.0 ) - t ) * omega) / sin_omega;
415         scale_to = sin(t * omega) / sin_omega;
416     }else{
417         scale_from = T( 1.0 ) - t;
418         scale_to = t;
419     }
420 
421     return ( from * scale_from ) + ( quatTo * scale_to  );
422 }
423 unittest{
424     auto v = armos.math.Vector3d(1, 0, 0);
425     auto qBegin= Quaternion!(double).angleAxis(0, armos.math.Vector3d(0, 0, 1));
426     auto qEnd = Quaternion!(double).angleAxis(PI*0.5, armos.math.Vector3d(0, 0, 1));
427     auto qSlerped = qBegin.slerp(qEnd, 2.0);
428     auto vR = qSlerped.rotatedVector(v);
429     auto vA = armos.math.Vector3d(-1, 0, 0);
430 
431     foreach (int i, value; vR.elements) {
432         assert( approxEqual(value, vA[i]) );
433     }
434 
435 }
436 
437 alias Quaternion!(float) Quaternionf;
438 alias Quaternion!(double) Quaterniond;