29 #include "../WAssert.h"
30 #include "../WLimits.h"
32 #include "WLinearAlgebraFunctions.h"
34 #include "linearAlgebra/WLinearAlgebra.h"
39 result[0] = mat( 0, 0 ) * vec[0] + mat( 0, 1 ) * vec[1] + mat( 0, 2 ) * vec[2];
40 result[1] = mat( 1, 0 ) * vec[0] + mat( 1, 1 ) * vec[1] + mat( 1, 2 ) * vec[2];
41 result[2] = mat( 2, 0 ) * vec[0] + mat( 2, 1 ) * vec[1] + mat( 2, 2 ) * vec[2];
48 std::vector< double > resultVec4D( 4 );
49 resultVec4D[0] = mat( 0, 0 ) * vec[0] + mat( 0, 1 ) * vec[1] + mat( 0, 2 ) * vec[2] ;
50 resultVec4D[1] = mat( 1, 0 ) * vec[0] + mat( 1, 1 ) * vec[1] + mat( 1, 2 ) * vec[2] ;
51 resultVec4D[2] = mat( 2, 0 ) * vec[0] + mat( 2, 1 ) * vec[1] + mat( 2, 2 ) * vec[2] ;
52 resultVec4D[3] = mat( 3, 0 ) * vec[0] + mat( 3, 1 ) * vec[1] + mat( 3, 2 ) * vec[2] ;
55 result[0] = resultVec4D[0] / resultVec4D[3];
56 result[1] = resultVec4D[1] / resultVec4D[3];
57 result[2] = resultVec4D[2] / resultVec4D[3];
64 std::vector< double > resultVec4D( 4 );
65 resultVec4D[0] = mat( 0, 0 ) * vec[0] + mat( 0, 1 ) * vec[1] + mat( 0, 2 ) * vec[2] + mat( 0, 3 ) * 1;
66 resultVec4D[1] = mat( 1, 0 ) * vec[0] + mat( 1, 1 ) * vec[1] + mat( 1, 2 ) * vec[2] + mat( 1, 3 ) * 1;
67 resultVec4D[2] = mat( 2, 0 ) * vec[0] + mat( 2, 1 ) * vec[1] + mat( 2, 2 ) * vec[2] + mat( 2, 3 ) * 1;
68 resultVec4D[3] = mat( 3, 0 ) * vec[0] + mat( 3, 1 ) * vec[1] + mat( 3, 2 ) * vec[2] + mat( 3, 3 ) * 1;
71 result[0] = resultVec4D[0] / resultVec4D[3];
72 result[1] = resultVec4D[1] / resultVec4D[3];
73 result[2] = resultVec4D[2] / resultVec4D[3];
79 WAssert( mat.
getNbRows(),
"Zero rows found." );
80 WAssert( mat.
getNbCols(),
"Zero columns found." );
81 double det = mat( 0, 0 ) * mat( 1, 1 ) * mat( 2, 2 ) +
82 mat( 0, 1 ) * mat( 1, 2 ) * mat( 2, 0 ) +
83 mat( 0, 2 ) * mat( 1, 0 ) * mat( 2, 1 ) -
84 mat( 0, 2 ) * mat( 1, 1 ) * mat( 2, 0 ) -
85 mat( 0, 1 ) * mat( 1, 0 ) * mat( 2, 2 ) -
86 mat( 0, 0 ) * mat( 1, 2 ) * mat( 2, 1 );
88 WAssert( det != 0,
"Determinant is zero. This matrix can not be inverted." );
92 r( 0, 0 ) = ( mat( 1, 1 ) * mat( 2, 2 ) - mat( 1, 2 ) * mat( 2, 1 ) ) / det;
93 r( 1, 0 ) = ( mat( 1, 2 ) * mat( 2, 0 ) - mat( 1, 0 ) * mat( 2, 2 ) ) / det;
94 r( 2, 0 ) = ( mat( 1, 0 ) * mat( 2, 1 ) - mat( 1, 1 ) * mat( 2, 0 ) ) / det;
95 r( 0, 1 ) = ( mat( 0, 2 ) * mat( 2, 1 ) - mat( 0, 1 ) * mat( 2, 2 ) ) / det;
96 r( 1, 1 ) = ( mat( 0, 0 ) * mat( 2, 2 ) - mat( 0, 2 ) * mat( 2, 0 ) ) / det;
97 r( 2, 1 ) = ( mat( 0, 1 ) * mat( 2, 0 ) - mat( 0, 0 ) * mat( 2, 1 ) ) / det;
98 r( 0, 2 ) = ( mat( 0, 1 ) * mat( 1, 2 ) - mat( 0, 2 ) * mat( 1, 1 ) ) / det;
99 r( 1, 2 ) = ( mat( 0, 2 ) * mat( 1, 0 ) - mat( 0, 0 ) * mat( 1, 2 ) ) / det;
100 r( 2, 2 ) = ( mat( 0, 0 ) * mat( 1, 1 ) - mat( 0, 1 ) * mat( 1, 0 ) ) / det;
107 WAssert( mat.
getNbRows(),
"Zero rows found." );
108 WAssert( mat.
getNbCols(),
"Zero columns found." );
110 mat( 0, 0 ) * mat( 1, 1 ) * mat( 2, 2 ) * mat( 3, 3 ) +
111 mat( 0, 0 ) * mat( 1, 2 ) * mat( 2, 3 ) * mat( 3, 1 ) +
112 mat( 0, 0 ) * mat( 1, 3 ) * mat( 2, 1 ) * mat( 3, 2 ) +
114 mat( 0, 1 ) * mat( 1, 0 ) * mat( 2, 3 ) * mat( 3, 2 ) +
115 mat( 0, 1 ) * mat( 1, 2 ) * mat( 2, 0 ) * mat( 3, 3 ) +
116 mat( 0, 1 ) * mat( 1, 3 ) * mat( 2, 2 ) * mat( 3, 0 ) +
118 mat( 0, 2 ) * mat( 1, 0 ) * mat( 2, 1 ) * mat( 3, 3 ) +
119 mat( 0, 2 ) * mat( 1, 1 ) * mat( 2, 3 ) * mat( 3, 0 ) +
120 mat( 0, 2 ) * mat( 1, 3 ) * mat( 2, 0 ) * mat( 3, 1 ) +
122 mat( 0, 3 ) * mat( 1, 0 ) * mat( 2, 2 ) * mat( 3, 1 ) +
123 mat( 0, 3 ) * mat( 1, 1 ) * mat( 2, 0 ) * mat( 3, 2 ) +
124 mat( 0, 3 ) * mat( 1, 2 ) * mat( 2, 1 ) * mat( 3, 0 ) -
126 mat( 0, 0 ) * mat( 1, 1 ) * mat( 2, 3 ) * mat( 3, 2 ) -
127 mat( 0, 0 ) * mat( 1, 2 ) * mat( 2, 1 ) * mat( 3, 3 ) -
128 mat( 0, 0 ) * mat( 1, 3 ) * mat( 2, 2 ) * mat( 3, 1 ) -
130 mat( 0, 1 ) * mat( 1, 0 ) * mat( 2, 2 ) * mat( 3, 3 ) -
131 mat( 0, 1 ) * mat( 1, 2 ) * mat( 2, 3 ) * mat( 3, 0 ) -
132 mat( 0, 1 ) * mat( 1, 3 ) * mat( 2, 0 ) * mat( 3, 2 ) -
134 mat( 0, 2 ) * mat( 1, 0 ) * mat( 2, 3 ) * mat( 3, 1 ) -
135 mat( 0, 2 ) * mat( 1, 1 ) * mat( 2, 0 ) * mat( 3, 3 ) -
136 mat( 0, 2 ) * mat( 1, 3 ) * mat( 2, 1 ) * mat( 3, 0 ) -
138 mat( 0, 3 ) * mat( 1, 0 ) * mat( 2, 1 ) * mat( 3, 2 ) -
139 mat( 0, 3 ) * mat( 1, 1 ) * mat( 2, 2 ) * mat( 3, 0 ) -
140 mat( 0, 3 ) * mat( 1, 2 ) * mat( 2, 0 ) * mat( 3, 1 );
145 mat( 1, 1 ) * mat( 2, 2 ) * mat( 3, 3 ) +
146 mat( 1, 2 ) * mat( 2, 3 ) * mat( 3, 1 ) +
147 mat( 1, 3 ) * mat( 2, 1 ) * mat( 3, 2 ) -
148 mat( 1, 1 ) * mat( 2, 3 ) * mat( 3, 2 ) -
149 mat( 1, 2 ) * mat( 2, 1 ) * mat( 3, 3 ) -
150 mat( 1, 3 ) * mat( 2, 2 ) * mat( 3, 1 );
153 mat( 0, 1 ) * mat( 2, 3 ) * mat( 3, 2 ) +
154 mat( 0, 2 ) * mat( 2, 1 ) * mat( 3, 3 ) +
155 mat( 0, 3 ) * mat( 2, 2 ) * mat( 3, 1 ) -
156 mat( 0, 1 ) * mat( 2, 2 ) * mat( 3, 3 ) -
157 mat( 0, 2 ) * mat( 2, 3 ) * mat( 3, 1 ) -
158 mat( 0, 3 ) * mat( 2, 1 ) * mat( 3, 2 );
161 mat( 0, 1 ) * mat( 1, 2 ) * mat( 3, 3 ) +
162 mat( 0, 2 ) * mat( 1, 3 ) * mat( 3, 1 ) +
163 mat( 0, 3 ) * mat( 1, 1 ) * mat( 3, 2 ) -
164 mat( 0, 1 ) * mat( 1, 3 ) * mat( 3, 2 ) -
165 mat( 0, 2 ) * mat( 1, 1 ) * mat( 3, 3 ) -
166 mat( 0, 3 ) * mat( 1, 2 ) * mat( 3, 1 );
169 mat( 0, 1 ) * mat( 1, 3 ) * mat( 2, 2 ) +
170 mat( 0, 2 ) * mat( 1, 1 ) * mat( 2, 3 ) +
171 mat( 0, 3 ) * mat( 1, 2 ) * mat( 2, 1 ) -
172 mat( 0, 1 ) * mat( 1, 2 ) * mat( 2, 3 ) -
173 mat( 0, 2 ) * mat( 1, 3 ) * mat( 2, 1 ) -
174 mat( 0, 3 ) * mat( 1, 1 ) * mat( 2, 2 );
177 mat( 1, 0 ) * mat( 2, 3 ) * mat( 3, 2 ) +
178 mat( 1, 2 ) * mat( 2, 0 ) * mat( 3, 3 ) +
179 mat( 1, 3 ) * mat( 2, 2 ) * mat( 3, 0 ) -
180 mat( 1, 0 ) * mat( 2, 2 ) * mat( 3, 3 ) -
181 mat( 1, 2 ) * mat( 2, 3 ) * mat( 3, 0 ) -
182 mat( 1, 3 ) * mat( 2, 0 ) * mat( 3, 2 );
185 mat( 0, 0 ) * mat( 2, 2 ) * mat( 3, 3 ) +
186 mat( 0, 2 ) * mat( 2, 3 ) * mat( 3, 0 ) +
187 mat( 0, 3 ) * mat( 2, 0 ) * mat( 3, 2 ) -
188 mat( 0, 0 ) * mat( 2, 3 ) * mat( 3, 2 ) -
189 mat( 0, 2 ) * mat( 2, 0 ) * mat( 3, 3 ) -
190 mat( 0, 3 ) * mat( 2, 2 ) * mat( 3, 0 );
193 mat( 0, 0 ) * mat( 1, 3 ) * mat( 3, 2 ) +
194 mat( 0, 2 ) * mat( 1, 0 ) * mat( 3, 3 ) +
195 mat( 0, 3 ) * mat( 1, 2 ) * mat( 3, 0 ) -
196 mat( 0, 0 ) * mat( 1, 2 ) * mat( 3, 3 ) -
197 mat( 0, 2 ) * mat( 1, 3 ) * mat( 3, 0 ) -
198 mat( 0, 3 ) * mat( 1, 0 ) * mat( 3, 2 );
201 mat( 0, 0 ) * mat( 1, 2 ) * mat( 2, 3 ) +
202 mat( 0, 2 ) * mat( 1, 3 ) * mat( 2, 0 ) +
203 mat( 0, 3 ) * mat( 1, 0 ) * mat( 2, 2 ) -
204 mat( 0, 0 ) * mat( 1, 3 ) * mat( 2, 2 ) -
205 mat( 0, 2 ) * mat( 1, 0 ) * mat( 2, 3 ) -
206 mat( 0, 3 ) * mat( 1, 2 ) * mat( 2, 0 );
209 mat( 1, 0 ) * mat( 2, 1 ) * mat( 3, 3 ) +
210 mat( 1, 1 ) * mat( 2, 3 ) * mat( 3, 0 ) +
211 mat( 1, 3 ) * mat( 2, 0 ) * mat( 3, 1 ) -
212 mat( 1, 0 ) * mat( 2, 3 ) * mat( 3, 1 ) -
213 mat( 1, 1 ) * mat( 2, 0 ) * mat( 3, 3 ) -
214 mat( 1, 3 ) * mat( 2, 1 ) * mat( 3, 0 );
217 mat( 0, 0 ) * mat( 2, 3 ) * mat( 3, 1 ) +
218 mat( 0, 1 ) * mat( 2, 0 ) * mat( 3, 3 ) +
219 mat( 0, 3 ) * mat( 2, 1 ) * mat( 3, 0 ) -
220 mat( 0, 0 ) * mat( 2, 1 ) * mat( 3, 3 ) -
221 mat( 0, 1 ) * mat( 2, 3 ) * mat( 3, 0 ) -
222 mat( 0, 3 ) * mat( 2, 0 ) * mat( 3, 1 );
225 mat( 0, 0 ) * mat( 1, 1 ) * mat( 3, 3 ) +
226 mat( 0, 1 ) * mat( 1, 3 ) * mat( 3, 0 ) +
227 mat( 0, 3 ) * mat( 1, 0 ) * mat( 3, 1 ) -
228 mat( 0, 0 ) * mat( 1, 3 ) * mat( 3, 1 ) -
229 mat( 0, 1 ) * mat( 1, 0 ) * mat( 3, 3 ) -
230 mat( 0, 3 ) * mat( 1, 1 ) * mat( 3, 0 );
233 mat( 0, 0 ) * mat( 1, 3 ) * mat( 2, 1 ) +
234 mat( 0, 1 ) * mat( 1, 0 ) * mat( 2, 3 ) +
235 mat( 0, 3 ) * mat( 1, 1 ) * mat( 2, 0 ) -
236 mat( 0, 0 ) * mat( 1, 1 ) * mat( 2, 3 ) -
237 mat( 0, 1 ) * mat( 1, 3 ) * mat( 2, 0 ) -
238 mat( 0, 3 ) * mat( 1, 0 ) * mat( 2, 1 );
241 mat( 1, 0 ) * mat( 2, 2 ) * mat( 3, 1 ) +
242 mat( 1, 1 ) * mat( 2, 0 ) * mat( 3, 2 ) +
243 mat( 1, 2 ) * mat( 2, 1 ) * mat( 3, 0 ) -
244 mat( 1, 0 ) * mat( 2, 1 ) * mat( 3, 2 ) -
245 mat( 1, 1 ) * mat( 2, 2 ) * mat( 3, 0 ) -
246 mat( 1, 2 ) * mat( 2, 0 ) * mat( 3, 1 );
249 mat( 0, 0 ) * mat( 2, 1 ) * mat( 3, 2 ) +
250 mat( 0, 1 ) * mat( 2, 2 ) * mat( 3, 0 ) +
251 mat( 0, 2 ) * mat( 2, 0 ) * mat( 3, 1 ) -
252 mat( 0, 0 ) * mat( 2, 2 ) * mat( 3, 1 ) -
253 mat( 0, 1 ) * mat( 2, 0 ) * mat( 3, 2 ) -
254 mat( 0, 2 ) * mat( 2, 1 ) * mat( 3, 0 );
257 mat( 0, 0 ) * mat( 1, 2 ) * mat( 3, 1 ) +
258 mat( 0, 1 ) * mat( 1, 0 ) * mat( 3, 2 ) +
259 mat( 0, 2 ) * mat( 1, 1 ) * mat( 3, 0 ) -
260 mat( 0, 0 ) * mat( 1, 1 ) * mat( 3, 2 ) -
261 mat( 0, 1 ) * mat( 1, 2 ) * mat( 3, 0 ) -
262 mat( 0, 2 ) * mat( 1, 0 ) * mat( 3, 1 );
265 mat( 0, 0 ) * mat( 1, 1 ) * mat( 2, 2 ) +
266 mat( 0, 1 ) * mat( 1, 2 ) * mat( 2, 0 ) +
267 mat( 0, 2 ) * mat( 1, 0 ) * mat( 2, 1 ) -
268 mat( 0, 0 ) * mat( 1, 2 ) * mat( 2, 1 ) -
269 mat( 0, 1 ) * mat( 1, 0 ) * mat( 2, 2 ) -
270 mat( 0, 2 ) * mat( 1, 1 ) * mat( 2, 0 );
272 WAssert( det != 0,
"Determinat is zero. This matrix can not be inverted." );
274 double detInv = 1. / det;
275 for(
size_t r = 0; r < 4; ++r)
277 for(
size_t c = 0; c < 4; ++c )
279 result( c, r ) *= detInv;
301 Eigen::MatrixXd eigenA( A );
302 Eigen::JacobiSVD<Eigen::MatrixXd> svd( eigenA, Eigen::ComputeFullU | Eigen::ComputeFullV );
314 computeSVD( input, U, V, Svec );
321 S( i, i ) = ( Svec[ i ] == 0.0 ) ? 0.0 : 1.0 / Svec[ i ];