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