1 // Written in the D programming language.
2 /*
3 NYSL Version 0.9982
4
5 A. This software is "Everyone'sWare". It means:
6 Anybody who has this software can use it as if he/she is
7 the author.
8
9 A-1. Freeware. No fee is required.
10 A-2. You can freely redistribute this software.
11 A-3. You can freely modify this software. And the source
12 may be used in any software with no limitation.
13 A-4. When you release a modified version to public, you
14 must publish it with your name.
15
16 B. The author is not responsible for any kind of damages or loss
17 while using or misusing this software, which is distributed
18 "AS IS". No warranty of any kind is expressed or implied.
19 You use AT YOUR OWN RISK.
20
21 C. Copyrighted to Kazuki KOMATSU
22
23 D. Above three clauses are applied both to source and binary
24 form of this software.
25 */
26
27 /**
28 このモジュールでは、四元数を扱います。
29 */
30 module carbon.quaternion;
31
32 import carbon.linear;
33
34 import std.conv,
35 std.format,
36 std.functional,
37 std.math,
38 std.traits;
39
40
41 /**
42 四元数
43 */
44 Quaternion!(CommonType!(A, B, C, D)) quaternion(A, B, C, D)(A a, B b, C c, D d)
45 if(is(Quaternion!(CommonType!(A, B, C, D))))
46 {
47 typeof(return) dst;
48
49 dst.a = a;
50 dst.b = b;
51 dst.c = c;
52 dst.d = d;
53
54 return dst;
55 }
56
57
58 /// ditto
59 Quaternion!A quaternion(A)(A a)
60 if(is(Quaternion!A))
61 {
62 typeof(return) dst;
63
64 dst.a = a;
65 dst.b = 0;
66 dst.c = 0;
67 dst.d = 0;
68
69 return dst;
70 }
71
72
73 /// ditto
74 Quaternion!(ElementType!V) quaternion(V)(V v)
75 if(isVector!V)
76 {
77 typeof(return) dst;
78 dst._vec4 = v;
79 return dst;
80 }
81
82
83 /// ditto
84 Quaternion!(CommonType!(R, ElementType!V)) quaternion(R, V)(R r, V v)
85 if(is(Quaternion!(CommonType!(R, ElementType!V))))
86 {
87 typeof(return) dst;
88
89 dst.s = r;
90 dst.v = v;
91
92 return dst;
93 }
94
95
96 /// ditto
97 Quaternion!E quaternion(E)(E[] arr)
98 if(is(Quaternion!E))
99 in{
100 assert(arr.length == 4);
101 }
102 body{
103 typeof(return) dst;
104 dst._vec4.array[] = arr[];
105
106 return dst;
107 }
108
109
110 /// ditto
111 struct Quaternion(S)
112 if(isNotVectorOrMatrix!S)
113 {
114 this(E)(in Quaternion!E q)
115 if(is(E : S))
116 {
117 this = q;
118 }
119
120
121 this()(in SCVector!(int, 4) m)
122 {
123 this._vec4 = m;
124 }
125
126
127 ///
128 ref inout(S) opIndex(size_t i) pure nothrow @safe inout
129 in{
130 assert(i < 4);
131 }
132 body{
133 return _vec4[i];
134 }
135
136
137 ref inout(S) s() pure nothrow @safe @property inout { return _vec4[0]; }
138 ref inout(S) i() pure nothrow @safe @property inout { return _vec4[1]; }
139 ref inout(S) j() pure nothrow @safe @property inout { return _vec4[2]; }
140 ref inout(S) k() pure nothrow @safe @property inout { return _vec4[3]; }
141
142
143 alias a = s;
144 alias b = i;
145 alias c = j;
146 alias d = k;
147
148 alias w = a;
149 alias x = b;
150 alias y = c;
151 alias z = d;
152
153
154 @property
155 auto v(this T)() pure nothrow @safe
156 {
157 return _vec4.pref.swizzle.bcd;
158 }
159
160
161 @property
162 void v(V)(in V v)
163 {
164 foreach(i; 0 .. 3)
165 this._vec4[i + 1] = v[i];
166 }
167
168
169 @property
170 V asVec4(V = SCVector!(S, 4))() inout
171 {
172 V v = this._vec4;
173 return v;
174 }
175
176
177 auto opUnary(string op : "-", E)(in Quaternion!E q) const
178 {
179 return typeof(return)(typeof(typeof(return).init._vec4)(this._vec4 * -1));
180 }
181
182
183 Quaternion!(CommonType!(S, E)) opBinary(string op : "+", E)(in Quaternion!E q) const
184 if(!is(CommonType!(S, E) == void))
185 {
186 return typeof(return)(typeof(typeof(return).init._vec4)(this._vec4.pref + q._vec4));
187 }
188
189
190 Quaternion!(CommonType!(S, E)) opBinary(string op : "-", E)(in Quaternion!E q) const
191 if(!is(CommonType!(S, E) == void))
192 {
193 return typeof(return)(typeof(typeof(return).init._vec4)(this._vec4.pref - q._vec4));
194 }
195
196
197 Quaternion!(CommonType!(S, E)) opBinary(string op : "*", E)(in Quaternion!E q) const
198 if(!is(CommonType!(S, E) == void))
199 {
200 return quaternion(this.s * q.s - this.v.dot(q.v), (this.s * q.v) + (q.s * this.v) + (this.v.cross(q.v)));
201 }
202
203
204 auto opBinary(string op : "/", E)(in Quaternion!E q) const
205 if(isFloatingPoint!(CommonType!(S, E)))
206 {
207 return this * q.inverse;
208 }
209
210
211 Quaternion!(CommonType!(S, E)) opBinary(string op : "+", E)(in E s) const
212 if(!is(CommonType!(S, E) == void))
213 {
214 typeof(return) dst = this;
215 dst.a += s;
216 return dst;
217 }
218
219
220 Quaternion!(CommonType!(S, E)) opBinary(string op : "-", E)(in E s) const
221 if(!is(CommonType!(S, E) == void))
222 {
223 typeof(return) dst = this;
224 dst.a -= s;
225 return dst;
226 }
227
228
229 Quaternion!(CommonType!(S, E)) opBinary(string op : "*", E)(in E s) const
230 if(!is(CommonType!(S, E) == void))
231 {
232 typeof(return) dst = this;
233 dst._vec4 *= s;
234 return dst;
235 }
236
237
238 Quaternion!(CommonType!(S, E)) opBinary(string op : "/", E)(in E s) const
239 if(!is(CommonType!(S, E) == void))
240 {
241 typeof(return) dst = this;
242 dst._vec4 /= s;
243 return dst;
244 }
245
246
247 Quaternion!(Select!(isFloatingPoint!S, S, real)) opBinary(string op : "^^", Int : long)(Int n) const
248 {
249 if(n < 0)
250 return this.inverse ^^ (-n);
251 else
252 {
253 typeof(return) dst = this, memo = this;
254
255 n >>= 1;
256 while(n){
257 memo *= memo;
258
259 if(n)
260 dst *= memo;
261
262 n >>= 1;
263 }
264
265 return dst;
266 }
267 }
268
269
270 Quaternion!(CommonType!(S, E)) opBinaryRight(string op : "+", E)(in E s) const
271 if(!is(CommonType!(S, E) == void))
272 {
273 typeof(return) dst;
274 dst = this;
275 dst.a += s;
276 return dst;
277 }
278
279
280 Quaternion!(CommonType!(S, E)) opBinaryRight(string op : "-", E)(in E s) const
281 if(!is(CommonType!(S, E) == void))
282 {
283 return quaternion!(CommonType!(S, E))(s) - this;
284 }
285
286
287 Quaternion!(CommonType!(S, E)) opBinaryRight(string op : "*", E)(in E s) const
288 if(!is(CommonType!(S, E) == void))
289 {
290 typeof(return) dst;
291 dst = this;
292 dst._vec4 *= s;
293 return dst;
294 }
295
296
297 auto opBinaryRight(string op : "/", E)(in E s) const
298 if(isFloatingPoint!(CommonType!(S, E)))
299 {
300 return s / this.sumOfSquare * this.conj;
301 }
302
303
304 void opAssign(E)(in Quaternion!E q)
305 if(is(E : S))
306 {
307 this._vec4 = q._vec4;
308 }
309
310
311 void opAssign(E)(in E s)
312 if(is(E : S))
313 {
314 this._vec4 = 0;
315 this.a = s;
316 }
317
318
319 void opOpAssign(string op, E)(in Quaternion!E q)
320 if(!is(CommonType!(S, E) == void) && is(typeof(mixin("this " ~ op ~ " q"))))
321 {
322 this = mixin("this " ~ op ~ " q");
323 }
324
325
326 void opOpAssign(string op, E)(in E s)
327 if(is(E : S) && is(typeof(mixin("this " ~ op ~ " s"))))
328 {
329 this = mixin("this " ~ op ~ " s");
330 }
331
332
333 void toString(scope void delegate(const(char)[]) sink, string formatString) const
334 {
335 formattedWrite(sink, formatString, _vec4.array);
336 }
337
338
339 bool opEquals(E)(auto ref const Quaternion!E q) pure nothrow @safe const
340 {
341 foreach(i; 0 .. 4)
342 if(this[i] != q[i])
343 return false;
344 return true;
345 }
346
347
348 private:
349 SCVector!(S, 4) _vec4 = [1, 0, 0, 0].matrix!(4, 1);
350 }
351
352
353 ///
354 unittest {
355 assert(Quaternion!int.init == quaternion(1, 0, 0, 0));
356 // 1 = [1; (0, 0, 0)]な四元数の作成
357 auto q = quaternion(1);
358
359 // 添字によるアクセス
360 assert(q[0] == 1);
361 assert(q[1] == 0);
362 assert(q[2] == 0);
363 assert(q[3] == 0);
364
365
366 // 1 + 2i + 3j + 4k = [1; (2, 3, 4)]な四元数の作成
367 q = quaternion(1, 2, 3, 4);
368 assert(q[0] == 1);
369 assert(q[1] == 2);
370 assert(q[2] == 3);
371 assert(q[3] == 4);
372
373 // a, b, c, dによるアクセス
374 assert(q.a == 1);
375 assert(q.b == 2);
376 assert(q.c == 3);
377 assert(q.d == 4);
378
379 // スカラー部であるs, ベクトル部であるvによるアクセス
380 assert(q.s == 1);
381 assert(q.v == [2, 3, 4].matrix!(3, 1));
382
383 // v = (i, j, k)
384 assert(q.i == 2);
385 assert(q.j == 3);
386 assert(q.k == 4);
387
388 // opIndexやa, b, c, d, i, j, k, s, vへは代入可能
389 q.s = 7;
390 assert(q[0] == 7);
391
392 // vはベクトルなので、ベクトルを代入可能
393 q.v = [4, 5, 6].matrix!(3, 1);
394 assert(q[1] == 4);
395 assert(q[2] == 5);
396 assert(q[3] == 6);
397
398 // スカラー部とベクトル部による四元数の作成
399 q = quaternion(8, [9, 10, 11].matrix!(3, 1));
400 assert(q[0] == 8);
401 assert(q[1] == 9);
402 assert(q[2] == 10);
403 assert(q[3] == 11);
404
405
406 // 和
407 q = quaternion(1, 2, 3, 4) + quaternion(2, 2, 2, 2);
408 assert(q == quaternion(3, 4, 5, 6));
409
410 q = q + 3;
411 assert(q == quaternion(6, 4, 5, 6));
412
413 q = 3 + q;
414 assert(q == quaternion(9, 4, 5, 6));
415
416 // 複合代入和
417 q += q;
418 assert(q == quaternion(18, 8, 10, 12));
419
420 q += 3;
421 assert(q == quaternion(21, 8, 10, 12));
422
423
424 // 差
425 q = quaternion(1, 2, 3, 4) - quaternion(2, 2, 2, 2);
426 assert(q == quaternion(-1, 0, 1, 2));
427
428 q = q - 3;
429 assert(q == quaternion(-4, 0, 1, 2));
430
431 q = 3 - q;
432 assert(q == quaternion(7, 0, -1, -2));
433
434 // 複合代入和
435 q -= q;
436 assert(q == quaternion(0, 0, 0, 0));
437
438 q -= 3;
439 assert(q == quaternion(-3, 0, 0, 0));
440
441
442 // 積
443 q = quaternion(1, 2, 3, 4) * quaternion(7, 6, 7, 8);
444 assert(q == quaternion(-58, 16, 36, 32));
445
446 q = quaternion(1, 2, 3, 4) * 4;
447 assert(q == quaternion(4, 8, 12, 16));
448
449 q = 4 * quaternion(1, 2, 3, 4);
450 assert(q == quaternion(4, 8, 12, 16));
451
452 q = quaternion(1, 2, 3, 4);
453 q *= quaternion(7, 6, 7, 8);
454 assert(q == quaternion(-58, 16, 36, 32));
455
456 q = quaternion(1, 2, 3, 4);
457 q *= 4;
458 assert(q == quaternion(4, 8, 12, 16));
459
460
461 // 商
462 assert((quaternion(-58.0, 16, 36, 32) / quaternion(7, 6, 7, 8)).approxEqual(quaternion(1, 2, 3, 4)));
463 assert(quaternion(4.0, 8, 12, 16) / 4 == quaternion(1, 2, 3, 4));
464 assert((16.0 / quaternion(1.0, 2, 3, 4)).approxEqual(quaternion(16.0) / quaternion(1.0, 2, 3, 4)));
465 auto p = quaternion(-58.0, 16, 36, 32);
466 p /= quaternion(7, 6, 7, 8);
467 assert(p.approxEqual(quaternion(1, 2, 3, 4)));
468
469 p = quaternion(4.0, 8, 12, 16);
470 p /= 4;
471 assert(p.approxEqual(quaternion(1, 2, 3, 4)));
472
473 // 累乗
474 q = quaternion(1, 1, 2, 2);
475 p = q ^^ 3;
476 assert(approxEqual(p, q * q * q));
477
478 p = q ^^ -3;
479 assert(approxEqual(p, q.inverse * q.inverse * q.inverse));
480 assert(approxEqual(q ^^ 3 * q ^^ -3, quaternion(1, 0, 0, 0)));
481 }
482
483
484 /**
485 四元数の各要素の自乗和を返します
486 */
487 auto sumOfSquare(E)(in Quaternion!E q)
488 {
489 return q.a ^^ 2 + q.b ^^ 2 + q.c ^^ 2 + q.d ^^ 2;
490 }
491
492 unittest
493 {
494 assert(quaternion(1, 2, 3, 4).sumOfSquare == 30);
495 }
496
497
498 /**
499 四元数の絶対値を返します
500 */
501 auto abs(E)(in Quaternion!E q)
502 {
503 static if(isFloatingPoint!E)
504 return q.sumOfSquare.sqrt;
505 else
506 return sqrt(q.sumOfSquare.to!real);
507 }
508
509 unittest
510 {
511 assert(quaternion(2, 2, 2, 2).abs == 4);
512 assert(quaternion(1, 2, 3, 4).abs == sqrt(30.0));
513 }
514
515
516 /**
517 四元数の共役を返します
518 */
519 Quaternion!E conj(E)(in Quaternion!E q) pure nothrow @safe
520 {
521 typeof(return) dst;
522 dst.s = q.s;
523 dst.v = q.v * -1;
524 return dst;
525 }
526
527 unittest
528 {
529 auto q = quaternion(1, 2, 3, 4);
530 assert(q.conj == quaternion(1, -2, -3, -4));
531 }
532
533
534 /**
535 approxEqualの四元数バージョン
536 */
537 bool approxEqual(alias pred = std.math.approxEqual, E1, E2)(in Quaternion!E1 q1, in Quaternion!E2 q2)
538 {
539 foreach(i; 0 .. 4)
540 if(!binaryFun!pred(q1[i], q2[i]))
541 return false;
542 return true;
543 }
544
545 unittest
546 {
547 auto q = quaternion(1, 2, 3, 4);
548 assert(approxEqual(q, q));
549 }
550
551
552 /**
553 正規化します
554 */
555 auto normalize(E)(in Quaternion!E q)
556 {
557 return q / q.abs;
558 }
559
560 unittest
561 {
562 auto q = quaternion(1, 2, 3, 4);
563 assert(std.math.approxEqual(q.normalize.sumOfSquare, 1));
564 }
565 import std.stdio;
566
567 /**
568 積の逆元
569 */
570 auto inverse(E)(in Quaternion!E q)
571 {
572 static if(!isFloatingPoint!E)
573 return q.conj / q.sumOfSquare.to!real;
574 else
575 return q.conj / q.sumOfSquare;
576 }
577
578 unittest
579 {
580 import std.stdio;
581
582 auto q = quaternion(1, 2, 3, 4);
583 assert(approxEqual((q * q.inverse), quaternion(1, 0, 0, 0)));
584 }