CPPLapack
 All Classes Files Functions Variables Friends Pages
dgematrix_small-specialized.hpp
Go to the documentation of this file.
1 //=============================================================================
2 /*! calculate inverse */
3 inline dgemat1 inv(const dgemat1& A)
4 {CPPL_VERBOSE_REPORT;
5  dgemat1 Ainv;
6  Ainv(0,0) =1./A(0,0);
7  return Ainv;
8 }
9 
10 ///////////////////////////////////////////////////////////////////////////////
11 ///////////////////////////////////////////////////////////////////////////////
12 ///////////////////////////////////////////////////////////////////////////////
13 ///////////////////////////////////////////////////////////////////////////////
14 ///////////////////////////////////////////////////////////////////////////////
15 ///////////////////////////////////////////////////////////////////////////////
16 ///////////////////////////////////////////////////////////////////////////////
17 ///////////////////////////////////////////////////////////////////////////////
18 ///////////////////////////////////////////////////////////////////////////////
19 
20 //=============================================================================
21 /*! calculate determinant */
22 inline double det(const dgemat2& A)
23 {CPPL_VERBOSE_REPORT;
24  return A(0,0)*A(1,1)-A(0,1)*A(1,0);
25 }
26 
27 //=============================================================================
28 /*! calculate inverse */
29 inline dgemat2 inv(const dgemat2& A)
30 {CPPL_VERBOSE_REPORT;
31  const double Adet( det(A) );
32  dgemat2 Ainv;
33  Ainv(0,0)= A(1,1)/Adet; Ainv(0,1)=-A(0,1)/Adet;
34  Ainv(1,0)=-A(1,0)/Adet; Ainv(1,1)= A(0,0)/Adet;
35  return Ainv;
36 }
37 
38 //=============================================================================
39 /*! return rotated tensor */
40 inline dgemat2 rotate(const dgemat2& m, const double& theta)
41 {CPPL_VERBOSE_REPORT;
42  //dgemat2 R(t2m(theta)); return R*m*t(R);//too slow
43  double c(std::cos(theta)), s(std::sin(theta));
44  double cc(c*c), cs(c*s), ss(s*s);
45  dgemat2 mat;
46  mat(0,0) =m(0,0)*cc -(m(0,1)+m(1,0))*cs +m(1,1)*ss;
47  mat(0,1) =m(0,1)*cc +(m(0,0)-m(1,1))*cs -m(1,0)*ss;
48  mat(1,0) =m(1,0)*cc +(m(0,0)-m(1,1))*cs -m(0,1)*ss;
49  mat(1,1) =m(1,1)*cc +(m(0,1)+m(1,0))*cs +m(0,0)*ss;
50  return mat;
51 }
52 
53 //=============================================================================
54 /*! convert theta to 2x2 rotational matrix */
55 inline dgemat2 t2m(const double& theta)
56 {CPPL_VERBOSE_REPORT;
57  dgemat2 R;
58  R(0,0)=std::cos(theta); R(0,1)=-std::sin(theta);
59  R(1,0)=std::sin(theta); R(1,1)= std::cos(theta);
60  return R;
61 }
62 
63 ///////////////////////////////////////////////////////////////////////////////
64 ///////////////////////////////////////////////////////////////////////////////
65 ///////////////////////////////////////////////////////////////////////////////
66 ///////////////////////////////////////////////////////////////////////////////
67 ///////////////////////////////////////////////////////////////////////////////
68 ///////////////////////////////////////////////////////////////////////////////
69 ///////////////////////////////////////////////////////////////////////////////
70 ///////////////////////////////////////////////////////////////////////////////
71 ///////////////////////////////////////////////////////////////////////////////
72 
73 //=============================================================================
74 /*! calculate determinant */
75 inline double det(const dgemat3& A)
76 {CPPL_VERBOSE_REPORT;
77  return
78  +A(0,0)*A(1,1)*A(2,2) -A(0,0)*A(1,2)*A(2,1)
79  +A(0,1)*A(1,2)*A(2,0) -A(0,1)*A(1,0)*A(2,2)
80  +A(0,2)*A(1,0)*A(2,1) -A(0,2)*A(1,1)*A(2,0);
81 }
82 
83 //=============================================================================
84 /*! calculate the 2nd invariant */
85 inline double II(const dgemat3& A)
86 {CPPL_VERBOSE_REPORT;
87  return
88  +A(0,0)*A(1,1) +A(1,1)*A(2,2) +A(2,2)*A(0,0)
89  -A(0,1)*A(1,0) -A(1,2)*A(2,1) -A(2,0)*A(0,2);
90 }
91 
92 //=============================================================================
93 /*! calculate inverse */
94 inline dgemat3 inv(const dgemat3& A)
95 {CPPL_VERBOSE_REPORT;
96  const double Adet( det(A) );
97  dgemat3 Ainv;
98  Ainv(0,0) =(A(1,1)*A(2,2)-A(1,2)*A(2,1))/Adet;
99  Ainv(0,1) =(A(0,2)*A(2,1)-A(0,1)*A(2,2))/Adet;
100  Ainv(0,2) =(A(0,1)*A(1,2)-A(0,2)*A(1,1))/Adet;
101  Ainv(1,0) =(A(1,2)*A(2,0)-A(1,0)*A(2,2))/Adet;
102  Ainv(1,1) =(A(0,0)*A(2,2)-A(0,2)*A(2,0))/Adet;
103  Ainv(1,2) =(A(0,2)*A(1,0)-A(0,0)*A(1,2))/Adet;
104  Ainv(2,0) =(A(1,0)*A(2,1)-A(1,1)*A(2,0))/Adet;
105  Ainv(2,1) =(A(0,1)*A(2,0)-A(0,0)*A(2,1))/Adet;
106  Ainv(2,2) =(A(0,0)*A(1,1)-A(0,1)*A(1,0))/Adet;
107  return Ainv;
108 }
109 
110 //=============================================================================
111 /*! rotate 3D matrix by quaternion */
112 inline dgemat3 rotate(const dgemat3& m, const dquater& q)
113 {CPPL_VERBOSE_REPORT;
114  dgemat3 R(q2m(q));
115  return R*m*t(R);
116 }
117 
118 //=============================================================================
119 /*! convert 3D rotational matrix to quaternion */
120 inline dquater m2q(const dgemat3& m)
121 {CPPL_VERBOSE_REPORT;
122  dcovec3 v( m(2,1)-m(1,2), m(0,2)-m(2,0), m(1,0)-m(0,1) );
123  double t( std::acos(0.5*(m(0,0)+m(1,1)+m(2,2)-1.)) );
124  return vt2q(v,t);
125 }
126 
127 ///////////////////////////////////////////////////////////////////////////////
128 ///////////////////////////////////////////////////////////////////////////////
129 ///////////////////////////////////////////////////////////////////////////////
130 ///////////////////////////////////////////////////////////////////////////////
131 ///////////////////////////////////////////////////////////////////////////////
132 ///////////////////////////////////////////////////////////////////////////////
133 ///////////////////////////////////////////////////////////////////////////////
134 ///////////////////////////////////////////////////////////////////////////////
135 ///////////////////////////////////////////////////////////////////////////////
136 
137 //=============================================================================
138 /*! calculate determinant */
139 inline double det(const dgemat4& A)
140 {CPPL_VERBOSE_REPORT;
141  return
142  ((A(0,0)*A(1,1)-A(0,1)*A(1,0))*A(2,2)+(A(0,2)*A(1,0)-A(0,0)*A(1,2))*A(2,1)+(A(0,1)*A(1,2)-A(0,2)*A(1,1))*A(2,0))*A(3,3)
143  +((A(0,1)*A(1,0)-A(0,0)*A(1,1))*A(2,3)+(A(0,0)*A(1,3)-A(0,3)*A(1,0))*A(2,1)+(A(0,3)*A(1,1)-A(0,1)*A(1,3))*A(2,0))*A(3,2)
144  +((A(0,0)*A(1,2)-A(0,2)*A(1,0))*A(2,3)+(A(0,3)*A(1,0)-A(0,0)*A(1,3))*A(2,2)+(A(0,2)*A(1,3)-A(0,3)*A(1,2))*A(2,0))*A(3,1)
145  +((A(0,2)*A(1,1)-A(0,1)*A(1,2))*A(2,3)+(A(0,1)*A(1,3)-A(0,3)*A(1,1))*A(2,2)+(A(0,3)*A(1,2)-A(0,2)*A(1,3))*A(2,1))*A(3,0);
146 }
147 
148 //=============================================================================
149 /*! calculate inverse */
150 inline dgemat4 inv(const dgemat4& A)
151 {CPPL_VERBOSE_REPORT;
152  const double Adet =det(A);
153 
154  dgemat4 Ainv;
155  Ainv(0,0) =(A(1,1)*A(2,2)-A(1,2)*A(2,1))*A(3,3)+(A(1,3)*A(2,1)-A(1,1)*A(2,3))*A(3,2)+(A(1,2)*A(2,3)-A(1,3)*A(2,2))*A(3,1);
156  Ainv(0,1) =(A(0,2)*A(2,1)-A(0,1)*A(2,2))*A(3,3)+(A(0,1)*A(2,3)-A(0,3)*A(2,1))*A(3,2)+(A(0,3)*A(2,2)-A(0,2)*A(2,3))*A(3,1);
157  Ainv(0,2) =(A(0,1)*A(1,2)-A(0,2)*A(1,1))*A(3,3)+(A(0,3)*A(1,1)-A(0,1)*A(1,3))*A(3,2)+(A(0,2)*A(1,3)-A(0,3)*A(1,2))*A(3,1);
158  Ainv(0,3) =(A(0,2)*A(1,1)-A(0,1)*A(1,2))*A(2,3)+(A(0,1)*A(1,3)-A(0,3)*A(1,1))*A(2,2)+(A(0,3)*A(1,2)-A(0,2)*A(1,3))*A(2,1);
159  Ainv(1,0) =(A(1,2)*A(2,0)-A(1,0)*A(2,2))*A(3,3)+(A(1,0)*A(2,3)-A(1,3)*A(2,0))*A(3,2)+(A(1,3)*A(2,2)-A(1,2)*A(2,3))*A(3,0);
160  Ainv(1,1) =(A(0,0)*A(2,2)-A(0,2)*A(2,0))*A(3,3)+(A(0,3)*A(2,0)-A(0,0)*A(2,3))*A(3,2)+(A(0,2)*A(2,3)-A(0,3)*A(2,2))*A(3,0);
161  Ainv(1,2) =(A(0,2)*A(1,0)-A(0,0)*A(1,2))*A(3,3)+(A(0,0)*A(1,3)-A(0,3)*A(1,0))*A(3,2)+(A(0,3)*A(1,2)-A(0,2)*A(1,3))*A(3,0);
162  Ainv(1,3) =(A(0,0)*A(1,2)-A(0,2)*A(1,0))*A(2,3)+(A(0,3)*A(1,0)-A(0,0)*A(1,3))*A(2,2)+(A(0,2)*A(1,3)-A(0,3)*A(1,2))*A(2,0);
163  Ainv(2,0) =(A(1,0)*A(2,1)-A(1,1)*A(2,0))*A(3,3)+(A(1,3)*A(2,0)-A(1,0)*A(2,3))*A(3,1)+(A(1,1)*A(2,3)-A(1,3)*A(2,1))*A(3,0);
164  Ainv(2,1) =(A(0,1)*A(2,0)-A(0,0)*A(2,1))*A(3,3)+(A(0,0)*A(2,3)-A(0,3)*A(2,0))*A(3,1)+(A(0,3)*A(2,1)-A(0,1)*A(2,3))*A(3,0);
165  Ainv(2,2) =(A(0,0)*A(1,1)-A(0,1)*A(1,0))*A(3,3)+(A(0,3)*A(1,0)-A(0,0)*A(1,3))*A(3,1)+(A(0,1)*A(1,3)-A(0,3)*A(1,1))*A(3,0);
166  Ainv(2,3) =(A(0,1)*A(1,0)-A(0,0)*A(1,1))*A(2,3)+(A(0,0)*A(1,3)-A(0,3)*A(1,0))*A(2,1)+(A(0,3)*A(1,1)-A(0,1)*A(1,3))*A(2,0);
167  Ainv(3,0) =(A(1,1)*A(2,0)-A(1,0)*A(2,1))*A(3,2)+(A(1,0)*A(2,2)-A(1,2)*A(2,0))*A(3,1)+(A(1,2)*A(2,1)-A(1,1)*A(2,2))*A(3,0);
168  Ainv(3,1) =(A(0,0)*A(2,1)-A(0,1)*A(2,0))*A(3,2)+(A(0,2)*A(2,0)-A(0,0)*A(2,2))*A(3,1)+(A(0,1)*A(2,2)-A(0,2)*A(2,1))*A(3,0);
169  Ainv(3,2) =(A(0,1)*A(1,0)-A(0,0)*A(1,1))*A(3,2)+(A(0,0)*A(1,2)-A(0,2)*A(1,0))*A(3,1)+(A(0,2)*A(1,1)-A(0,1)*A(1,2))*A(3,0);
170  Ainv(3,3) =(A(0,0)*A(1,1)-A(0,1)*A(1,0))*A(2,2)+(A(0,2)*A(1,0)-A(0,0)*A(1,2))*A(2,1)+(A(0,1)*A(1,2)-A(0,2)*A(1,1))*A(2,0);
171 
172  Ainv /=Adet;
173  return Ainv;
174 }
t2m
dgemat2 t2m(const double &theta)
Definition: dgematrix_small-specialized.hpp:55
det
double det(const dgemat2 &A)
Definition: dgematrix_small-specialized.hpp:22
m2q
dquater m2q(const dgemat3 &m)
Definition: dgematrix_small-specialized.hpp:120
vt2q
dquater vt2q(const dcovec3 &v, const double &theta)
Definition: dcovector_small-specialized.hpp:63
q2m
dgemat3 q2m(const dquater &q)
Definition: dcovector_small-specialized.hpp:161
II
double II(const dgemat3 &A)
Definition: dgematrix_small-specialized.hpp:85
t
drovector t(const _dcovector &covec)
Definition: _dcovector-calc.hpp:3
rotate
dgemat2 rotate(const dgemat2 &m, const double &theta)
Definition: dgematrix_small-specialized.hpp:40
inv
dgemat1 inv(const dgemat1 &A)
Definition: dgematrix_small-specialized.hpp:3
Generated by   doxygen 1.8.6