OpenWalnut  1.3.1
WLinearAlgebraFunctions.cpp
1 //---------------------------------------------------------------------------
2 //
3 // Project: OpenWalnut ( http://www.openwalnut.org )
4 //
5 // Copyright 2009 OpenWalnut Community, BSV@Uni-Leipzig and CNCF@MPI-CBS
6 // For more information see http://www.openwalnut.org/copying
7 //
8 // This file is part of OpenWalnut.
9 //
10 // OpenWalnut is free software: you can redistribute it and/or modify
11 // it under the terms of the GNU Lesser General Public License as published by
12 // the Free Software Foundation, either version 3 of the License, or
13 // (at your option) any later version.
14 //
15 // OpenWalnut is distributed in the hope that it will be useful,
16 // but WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 // GNU Lesser General Public License for more details.
19 //
20 // You should have received a copy of the GNU Lesser General Public License
21 // along with OpenWalnut. If not, see <http://www.gnu.org/licenses/>.
22 //
23 //---------------------------------------------------------------------------
24 
25 #include <vector>
26 
27 #include <Eigen/SVD>
28 
29 #include "../WAssert.h"
30 #include "../WLimits.h"
31 
32 #include "WLinearAlgebraFunctions.h"
33 #include "WMatrix.h"
34 #include "linearAlgebra/WLinearAlgebra.h"
35 
36 WVector3d multMatrixWithVector3D( WMatrix<double> mat, WVector3d vec )
37 {
38  WVector3d result;
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];
42  return result;
43 }
44 
45 WVector3d transformVector3DWithMatrix4D( WMatrix<double> mat, WVector3d vec )
46 {
47  WAssert( mat.getNbRows() == 4 && mat.getNbCols() == 4, "Matrix has wrong size." );
48  std::vector< double > resultVec4D( 4 );
49  resultVec4D[0] = mat( 0, 0 ) * vec[0] + mat( 0, 1 ) * vec[1] + mat( 0, 2 ) * vec[2] /* + mat( 0, 3 ) * 0 */;
50  resultVec4D[1] = mat( 1, 0 ) * vec[0] + mat( 1, 1 ) * vec[1] + mat( 1, 2 ) * vec[2] /* + mat( 1, 3 ) * 0 */;
51  resultVec4D[2] = mat( 2, 0 ) * vec[0] + mat( 2, 1 ) * vec[1] + mat( 2, 2 ) * vec[2] /* + mat( 2, 3 ) * 0 */;
52  resultVec4D[3] = mat( 3, 0 ) * vec[0] + mat( 3, 1 ) * vec[1] + mat( 3, 2 ) * vec[2] /* + mat( 3, 3 ) * 0 */;
53 
54  WVector3d result;
55  result[0] = resultVec4D[0] / resultVec4D[3];
56  result[1] = resultVec4D[1] / resultVec4D[3];
57  result[2] = resultVec4D[2] / resultVec4D[3];
58  return result;
59 }
60 
61 WPosition transformPosition3DWithMatrix4D( WMatrix<double> mat, WPosition vec )
62 {
63  WAssert( mat.getNbRows() == 4 && mat.getNbCols() == 4, "Matrix has wrong size." );
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;
69 
70  WPosition result;
71  result[0] = resultVec4D[0] / resultVec4D[3];
72  result[1] = resultVec4D[1] / resultVec4D[3];
73  result[2] = resultVec4D[2] / resultVec4D[3];
74  return result;
75 }
76 
77 WMatrix<double> invertMatrix3x3( WMatrix<double> mat )
78 {
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 );
87 
88  WAssert( det != 0, "Determinant is zero. This matrix can not be inverted." );
89 
90  WMatrix<double> r( 3, 3 );
91 
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;
101 
102  return r;
103 }
104 
105 WMatrix<double> invertMatrix4x4( WMatrix<double> mat )
106 {
107  WAssert( mat.getNbRows(), "Zero rows found." );
108  WAssert( mat.getNbCols(), "Zero columns found." );
109  double det =
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 ) +
113 
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 ) +
117 
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 ) +
121 
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 ) -
125 
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 ) -
129 
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 ) -
133 
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 ) -
137 
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 );
141 
142  WMatrix<double> result( 4, 4 );
143 
144  result( 0, 0 ) =
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 );
151 
152  result( 0, 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 );
159 
160  result( 0, 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 );
167 
168  result( 0, 3 ) =
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 );
175 
176  result( 1, 0 ) =
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 );
183 
184  result( 1, 1 ) =
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 );
191 
192  result( 1, 2 ) =
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 );
199 
200  result( 1, 3 ) =
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 );
207 
208  result( 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 );
215 
216  result( 2, 1 ) =
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 );
223 
224  result( 2, 2 ) =
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 );
231 
232  result( 2, 3 ) =
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 );
239 
240  result( 3, 0 ) =
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 );
247 
248  result( 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 );
255 
256  result( 3, 2 ) =
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 );
263 
264  result( 3, 3 ) =
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 );
271 
272  WAssert( det != 0, "Determinat is zero. This matrix can not be inverted." );
273 
274  double detInv = 1. / det;
275  for( size_t r = 0; r < 4; ++r)
276  {
277  for( size_t c = 0; c < 4; ++c )
278  {
279  result( c, r ) *= detInv;
280  }
281  }
282 
283  return result;
284 }
285 
286 bool linearIndependent( const WVector3d& u, const WVector3d& v )
287 {
288  WVector3d cp = cross( u, v );
289  if( std::fabs( cp[0] ) < wlimits::DBL_EPS && std::fabs( cp[1] ) < wlimits::DBL_EPS && std::fabs( cp[2] ) < wlimits::DBL_EPS )
290  {
291  return false;
292  }
293  return true;
294 }
295 
296 void computeSVD( const WMatrix<double>& A,
297  WMatrix<double>& U,
298  WMatrix<double>& V,
299  WValue<double>& S )
300 {
301  Eigen::MatrixXd eigenA( A );
302  Eigen::JacobiSVD<Eigen::MatrixXd> svd( eigenA, Eigen::ComputeFullU | Eigen::ComputeFullV );
303  U = WMatrix<double>( svd.matrixU() );
304  V = WMatrix<double>( svd.matrixV() );
305  S = WValue<double>( svd.singularValues() );
306 }
307 
308 WMatrix<double> pseudoInverse( const WMatrix<double>& input )
309 {
310  // calc pseudo inverse
311  WMatrix<double> U( input.getNbRows(), input.getNbCols() );
312  WMatrix<double> V( input.getNbCols(), input.getNbCols() );
313  WValue<double> Svec( input.size() );
314  computeSVD( input, U, V, Svec );
315 
316  // create diagonal matrix S
317  WMatrix<double> S( input.getNbCols(), input.getNbCols() );
318  S.setZero();
319  for( size_t i = 0; i < Svec.size() && i < S.getNbRows() && i < S.getNbCols(); i++ )
320  {
321  S( i, i ) = ( Svec[ i ] == 0.0 ) ? 0.0 : 1.0 / Svec[ i ];
322  }
323 
324  return WMatrix<double>( V*S*U.transposed() );
325 }