des.math.linear.quaterni source code

1 module des.math.linear.quaterni;
2 
3 import des.math.linear.vector;
4 import des.math.linear.matrix;
5 
6 import std.traits;
7 import std.exception : enforce;
8 import std.math;
9 
10 import des.util.testsuite;
11 
12 import des.math.util;
13 import des.math.basic;
14 
15 ///
16 struct Quaterni(T) if( isFloatingPoint!T )
17 {
18     ///
19     alias vectype = Vector!(4,T);
20     ///
21     vectype data;
22     ///
23     alias data this;
24     ///
25     alias selftype = Quaterni!T;
26 
27 pure:
28     ///
29     this(E...)( in E vals ) if( is( typeof( vectype(vals) ) ) )
30     { data = vectype(vals); }
31 
32     mixin accessByString!(4,T,"data.data","i j k a");
33     mixin( BasicMathOp!"data" );
34 
35     ///
36     static selftype fromAngle(size_t K,E)( T alpha, in Vector!(K,E) axis )
37         if( (K==0||K==3) && isFloatingPoint!E )
38     {
39         static if( K==0 ) enforce( axis.length == 3, "wrong length" );
40         T a = alpha / cast(T)(2.0);
41         auto vv = axis * sin(a);
42         return selftype( vv[0], vv[1], vv[2], cos(a) );
43     }
44 
45     ///
46     auto opMul(E)( in Quaterni!E b ) const
47     {
48         alias this a;
49         auto aijk = a.ijk;
50         auto bijk = b.ijk;
51         auto vv = cross( aijk, bijk ) + aijk * b.a + bijk * a.a;
52         return Quaterni!T( vv[0], vv[1], vv[2], a.a * b.a - dot(aijk, bijk) );
53     }
54 
55     ///
56     auto rot(size_t K,E)( in Vector!(K,E) b ) const
57         if( (K==0||K==3) && is( CommonType!(T,E) : T ) )
58     {
59         static if( K==0 ) enforce( b.length == 3, "wrong length" );
60         auto res = this * selftype(b,0) * inv;
61         return Vector!(K,T)( res.ijk );
62     }
63 
64     const @property
65     {
66         ///
67         T norm() { return dot( this, this ); }
68         ///
69         T mag() { return sqrt( norm ); }
70         ///
71         auto con() { return selftype( -this.ijk, this.a ); }
72         ///
73         auto inv() { return selftype( con / norm ); }
74 
75         auto len2() { return data.len2; }
76     }
77 }
78 
79 ///
80 alias Quaterni!float quat;
81 ///
82 alias Quaterni!double dquat;
83 ///
84 alias Quaterni!real rquat;
85 
86 ///
87 unittest
88 {
89     auto q = quat.fromAngle( PI_2, vec3(0,0,1) );
90     auto v = vec3(1,0,0);
91     auto e = vec3(0,1,0);
92     auto r = q.rot(v);
93     assert( eq( r, e ) );
94 }