OpenWalnut  1.5.0dev
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 "../WAssert.h"
28 #include "../WLimits.h"
29 
30 #include "WLinearAlgebraFunctions.h"
31 #include "WMatrix.h"
32 #include "linearAlgebra/WVectorFixed.h"
33 
34 WVector3d multMatrixWithVector3D( WMatrix<double> mat, WVector3d vec )
35 {
36  WVector3d result;
37  result[0] = mat( 0, 0 ) * vec[0] + mat( 0, 1 ) * vec[1] + mat( 0, 2 ) * vec[2];
38  result[1] = mat( 1, 0 ) * vec[0] + mat( 1, 1 ) * vec[1] + mat( 1, 2 ) * vec[2];
39  result[2] = mat( 2, 0 ) * vec[0] + mat( 2, 1 ) * vec[1] + mat( 2, 2 ) * vec[2];
40  return result;
41 }
42 
43 WVector3d transformVector3DWithMatrix4D( WMatrix<double> mat, WVector3d vec )
44 {
45  WAssert( mat.getNbRows() == 4 && mat.getNbCols() == 4, "Matrix has wrong size." );
46  std::vector< double > resultVec4D( 4 );
47  resultVec4D[0] = mat( 0, 0 ) * vec[0] + mat( 0, 1 ) * vec[1] + mat( 0, 2 ) * vec[2] /* + mat( 0, 3 ) * 0 */;
48  resultVec4D[1] = mat( 1, 0 ) * vec[0] + mat( 1, 1 ) * vec[1] + mat( 1, 2 ) * vec[2] /* + mat( 1, 3 ) * 0 */;
49  resultVec4D[2] = mat( 2, 0 ) * vec[0] + mat( 2, 1 ) * vec[1] + mat( 2, 2 ) * vec[2] /* + mat( 2, 3 ) * 0 */;
50  resultVec4D[3] = mat( 3, 0 ) * vec[0] + mat( 3, 1 ) * vec[1] + mat( 3, 2 ) * vec[2] /* + mat( 3, 3 ) * 0 */;
51 
52  WVector3d result;
53  result[0] = resultVec4D[0] / resultVec4D[3];
54  result[1] = resultVec4D[1] / resultVec4D[3];
55  result[2] = resultVec4D[2] / resultVec4D[3];
56  return result;
57 }
58 
59 WPosition transformPosition3DWithMatrix4D( WMatrix<double> mat, WPosition vec )
60 {
61  WAssert( mat.getNbRows() == 4 && mat.getNbCols() == 4, "Matrix has wrong size." );
62  std::vector< double > resultVec4D( 4 );
63  resultVec4D[0] = mat( 0, 0 ) * vec[0] + mat( 0, 1 ) * vec[1] + mat( 0, 2 ) * vec[2] + mat( 0, 3 ) * 1;
64  resultVec4D[1] = mat( 1, 0 ) * vec[0] + mat( 1, 1 ) * vec[1] + mat( 1, 2 ) * vec[2] + mat( 1, 3 ) * 1;
65  resultVec4D[2] = mat( 2, 0 ) * vec[0] + mat( 2, 1 ) * vec[1] + mat( 2, 2 ) * vec[2] + mat( 2, 3 ) * 1;
66  resultVec4D[3] = mat( 3, 0 ) * vec[0] + mat( 3, 1 ) * vec[1] + mat( 3, 2 ) * vec[2] + mat( 3, 3 ) * 1;
67 
68  WPosition result;
69  result[0] = resultVec4D[0] / resultVec4D[3];
70  result[1] = resultVec4D[1] / resultVec4D[3];
71  result[2] = resultVec4D[2] / resultVec4D[3];
72  return result;
73 }
74 
75 WMatrix<double> invertMatrix3x3( WMatrix<double> mat )
76 {
77  WAssert( mat.getNbRows(), "Zero rows found." );
78  WAssert( mat.getNbCols(), "Zero columns found." );
79  double det = mat( 0, 0 ) * mat( 1, 1 ) * mat( 2, 2 ) +
80  mat( 0, 1 ) * mat( 1, 2 ) * mat( 2, 0 ) +
81  mat( 0, 2 ) * mat( 1, 0 ) * mat( 2, 1 ) -
82  mat( 0, 2 ) * mat( 1, 1 ) * mat( 2, 0 ) -
83  mat( 0, 1 ) * mat( 1, 0 ) * mat( 2, 2 ) -
84  mat( 0, 0 ) * mat( 1, 2 ) * mat( 2, 1 );
85 
86  WAssert( det != 0, "Determinant is zero. This matrix can not be inverted." );
87 
88  WMatrix<double> r( 3, 3 );
89 
90  r( 0, 0 ) = ( mat( 1, 1 ) * mat( 2, 2 ) - mat( 1, 2 ) * mat( 2, 1 ) ) / det;
91  r( 1, 0 ) = ( mat( 1, 2 ) * mat( 2, 0 ) - mat( 1, 0 ) * mat( 2, 2 ) ) / det;
92  r( 2, 0 ) = ( mat( 1, 0 ) * mat( 2, 1 ) - mat( 1, 1 ) * mat( 2, 0 ) ) / det;
93  r( 0, 1 ) = ( mat( 0, 2 ) * mat( 2, 1 ) - mat( 0, 1 ) * mat( 2, 2 ) ) / det;
94  r( 1, 1 ) = ( mat( 0, 0 ) * mat( 2, 2 ) - mat( 0, 2 ) * mat( 2, 0 ) ) / det;
95  r( 2, 1 ) = ( mat( 0, 1 ) * mat( 2, 0 ) - mat( 0, 0 ) * mat( 2, 1 ) ) / det;
96  r( 0, 2 ) = ( mat( 0, 1 ) * mat( 1, 2 ) - mat( 0, 2 ) * mat( 1, 1 ) ) / det;
97  r( 1, 2 ) = ( mat( 0, 2 ) * mat( 1, 0 ) - mat( 0, 0 ) * mat( 1, 2 ) ) / det;
98  r( 2, 2 ) = ( mat( 0, 0 ) * mat( 1, 1 ) - mat( 0, 1 ) * mat( 1, 0 ) ) / det;
99 
100  return r;
101 }
102 
103 WMatrix<double> invertMatrix4x4( WMatrix<double> mat )
104 {
105  WAssert( mat.getNbRows(), "Zero rows found." );
106  WAssert( mat.getNbCols(), "Zero columns found." );
107  double det =
108  mat( 0, 0 ) * mat( 1, 1 ) * mat( 2, 2 ) * mat( 3, 3 ) +
109  mat( 0, 0 ) * mat( 1, 2 ) * mat( 2, 3 ) * mat( 3, 1 ) +
110  mat( 0, 0 ) * mat( 1, 3 ) * mat( 2, 1 ) * mat( 3, 2 ) +
111 
112  mat( 0, 1 ) * mat( 1, 0 ) * mat( 2, 3 ) * mat( 3, 2 ) +
113  mat( 0, 1 ) * mat( 1, 2 ) * mat( 2, 0 ) * mat( 3, 3 ) +
114  mat( 0, 1 ) * mat( 1, 3 ) * mat( 2, 2 ) * mat( 3, 0 ) +
115 
116  mat( 0, 2 ) * mat( 1, 0 ) * mat( 2, 1 ) * mat( 3, 3 ) +
117  mat( 0, 2 ) * mat( 1, 1 ) * mat( 2, 3 ) * mat( 3, 0 ) +
118  mat( 0, 2 ) * mat( 1, 3 ) * mat( 2, 0 ) * mat( 3, 1 ) +
119 
120  mat( 0, 3 ) * mat( 1, 0 ) * mat( 2, 2 ) * mat( 3, 1 ) +
121  mat( 0, 3 ) * mat( 1, 1 ) * mat( 2, 0 ) * mat( 3, 2 ) +
122  mat( 0, 3 ) * mat( 1, 2 ) * mat( 2, 1 ) * mat( 3, 0 ) -
123 
124  mat( 0, 0 ) * mat( 1, 1 ) * mat( 2, 3 ) * mat( 3, 2 ) -
125  mat( 0, 0 ) * mat( 1, 2 ) * mat( 2, 1 ) * mat( 3, 3 ) -
126  mat( 0, 0 ) * mat( 1, 3 ) * mat( 2, 2 ) * mat( 3, 1 ) -
127 
128  mat( 0, 1 ) * mat( 1, 0 ) * mat( 2, 2 ) * mat( 3, 3 ) -
129  mat( 0, 1 ) * mat( 1, 2 ) * mat( 2, 3 ) * mat( 3, 0 ) -
130  mat( 0, 1 ) * mat( 1, 3 ) * mat( 2, 0 ) * mat( 3, 2 ) -
131 
132  mat( 0, 2 ) * mat( 1, 0 ) * mat( 2, 3 ) * mat( 3, 1 ) -
133  mat( 0, 2 ) * mat( 1, 1 ) * mat( 2, 0 ) * mat( 3, 3 ) -
134  mat( 0, 2 ) * mat( 1, 3 ) * mat( 2, 1 ) * mat( 3, 0 ) -
135 
136  mat( 0, 3 ) * mat( 1, 0 ) * mat( 2, 1 ) * mat( 3, 2 ) -
137  mat( 0, 3 ) * mat( 1, 1 ) * mat( 2, 2 ) * mat( 3, 0 ) -
138  mat( 0, 3 ) * mat( 1, 2 ) * mat( 2, 0 ) * mat( 3, 1 );
139 
140  WMatrix<double> result( 4, 4 );
141 
142  result( 0, 0 ) =
143  mat( 1, 1 ) * mat( 2, 2 ) * mat( 3, 3 ) +
144  mat( 1, 2 ) * mat( 2, 3 ) * mat( 3, 1 ) +
145  mat( 1, 3 ) * mat( 2, 1 ) * mat( 3, 2 ) -
146  mat( 1, 1 ) * mat( 2, 3 ) * mat( 3, 2 ) -
147  mat( 1, 2 ) * mat( 2, 1 ) * mat( 3, 3 ) -
148  mat( 1, 3 ) * mat( 2, 2 ) * mat( 3, 1 );
149 
150  result( 0, 1 ) =
151  mat( 0, 1 ) * mat( 2, 3 ) * mat( 3, 2 ) +
152  mat( 0, 2 ) * mat( 2, 1 ) * mat( 3, 3 ) +
153  mat( 0, 3 ) * mat( 2, 2 ) * mat( 3, 1 ) -
154  mat( 0, 1 ) * mat( 2, 2 ) * mat( 3, 3 ) -
155  mat( 0, 2 ) * mat( 2, 3 ) * mat( 3, 1 ) -
156  mat( 0, 3 ) * mat( 2, 1 ) * mat( 3, 2 );
157 
158  result( 0, 2 ) =
159  mat( 0, 1 ) * mat( 1, 2 ) * mat( 3, 3 ) +
160  mat( 0, 2 ) * mat( 1, 3 ) * mat( 3, 1 ) +
161  mat( 0, 3 ) * mat( 1, 1 ) * mat( 3, 2 ) -
162  mat( 0, 1 ) * mat( 1, 3 ) * mat( 3, 2 ) -
163  mat( 0, 2 ) * mat( 1, 1 ) * mat( 3, 3 ) -
164  mat( 0, 3 ) * mat( 1, 2 ) * mat( 3, 1 );
165 
166  result( 0, 3 ) =
167  mat( 0, 1 ) * mat( 1, 3 ) * mat( 2, 2 ) +
168  mat( 0, 2 ) * mat( 1, 1 ) * mat( 2, 3 ) +
169  mat( 0, 3 ) * mat( 1, 2 ) * mat( 2, 1 ) -
170  mat( 0, 1 ) * mat( 1, 2 ) * mat( 2, 3 ) -
171  mat( 0, 2 ) * mat( 1, 3 ) * mat( 2, 1 ) -
172  mat( 0, 3 ) * mat( 1, 1 ) * mat( 2, 2 );
173 
174  result( 1, 0 ) =
175  mat( 1, 0 ) * mat( 2, 3 ) * mat( 3, 2 ) +
176  mat( 1, 2 ) * mat( 2, 0 ) * mat( 3, 3 ) +
177  mat( 1, 3 ) * mat( 2, 2 ) * mat( 3, 0 ) -
178  mat( 1, 0 ) * mat( 2, 2 ) * mat( 3, 3 ) -
179  mat( 1, 2 ) * mat( 2, 3 ) * mat( 3, 0 ) -
180  mat( 1, 3 ) * mat( 2, 0 ) * mat( 3, 2 );
181 
182  result( 1, 1 ) =
183  mat( 0, 0 ) * mat( 2, 2 ) * mat( 3, 3 ) +
184  mat( 0, 2 ) * mat( 2, 3 ) * mat( 3, 0 ) +
185  mat( 0, 3 ) * mat( 2, 0 ) * mat( 3, 2 ) -
186  mat( 0, 0 ) * mat( 2, 3 ) * mat( 3, 2 ) -
187  mat( 0, 2 ) * mat( 2, 0 ) * mat( 3, 3 ) -
188  mat( 0, 3 ) * mat( 2, 2 ) * mat( 3, 0 );
189 
190  result( 1, 2 ) =
191  mat( 0, 0 ) * mat( 1, 3 ) * mat( 3, 2 ) +
192  mat( 0, 2 ) * mat( 1, 0 ) * mat( 3, 3 ) +
193  mat( 0, 3 ) * mat( 1, 2 ) * mat( 3, 0 ) -
194  mat( 0, 0 ) * mat( 1, 2 ) * mat( 3, 3 ) -
195  mat( 0, 2 ) * mat( 1, 3 ) * mat( 3, 0 ) -
196  mat( 0, 3 ) * mat( 1, 0 ) * mat( 3, 2 );
197 
198  result( 1, 3 ) =
199  mat( 0, 0 ) * mat( 1, 2 ) * mat( 2, 3 ) +
200  mat( 0, 2 ) * mat( 1, 3 ) * mat( 2, 0 ) +
201  mat( 0, 3 ) * mat( 1, 0 ) * mat( 2, 2 ) -
202  mat( 0, 0 ) * mat( 1, 3 ) * mat( 2, 2 ) -
203  mat( 0, 2 ) * mat( 1, 0 ) * mat( 2, 3 ) -
204  mat( 0, 3 ) * mat( 1, 2 ) * mat( 2, 0 );
205 
206  result( 2, 0 ) =
207  mat( 1, 0 ) * mat( 2, 1 ) * mat( 3, 3 ) +
208  mat( 1, 1 ) * mat( 2, 3 ) * mat( 3, 0 ) +
209  mat( 1, 3 ) * mat( 2, 0 ) * mat( 3, 1 ) -
210  mat( 1, 0 ) * mat( 2, 3 ) * mat( 3, 1 ) -
211  mat( 1, 1 ) * mat( 2, 0 ) * mat( 3, 3 ) -
212  mat( 1, 3 ) * mat( 2, 1 ) * mat( 3, 0 );
213 
214  result( 2, 1 ) =
215  mat( 0, 0 ) * mat( 2, 3 ) * mat( 3, 1 ) +
216  mat( 0, 1 ) * mat( 2, 0 ) * mat( 3, 3 ) +
217  mat( 0, 3 ) * mat( 2, 1 ) * mat( 3, 0 ) -
218  mat( 0, 0 ) * mat( 2, 1 ) * mat( 3, 3 ) -
219  mat( 0, 1 ) * mat( 2, 3 ) * mat( 3, 0 ) -
220  mat( 0, 3 ) * mat( 2, 0 ) * mat( 3, 1 );
221 
222  result( 2, 2 ) =
223  mat( 0, 0 ) * mat( 1, 1 ) * mat( 3, 3 ) +
224  mat( 0, 1 ) * mat( 1, 3 ) * mat( 3, 0 ) +
225  mat( 0, 3 ) * mat( 1, 0 ) * mat( 3, 1 ) -
226  mat( 0, 0 ) * mat( 1, 3 ) * mat( 3, 1 ) -
227  mat( 0, 1 ) * mat( 1, 0 ) * mat( 3, 3 ) -
228  mat( 0, 3 ) * mat( 1, 1 ) * mat( 3, 0 );
229 
230  result( 2, 3 ) =
231  mat( 0, 0 ) * mat( 1, 3 ) * mat( 2, 1 ) +
232  mat( 0, 1 ) * mat( 1, 0 ) * mat( 2, 3 ) +
233  mat( 0, 3 ) * mat( 1, 1 ) * mat( 2, 0 ) -
234  mat( 0, 0 ) * mat( 1, 1 ) * mat( 2, 3 ) -
235  mat( 0, 1 ) * mat( 1, 3 ) * mat( 2, 0 ) -
236  mat( 0, 3 ) * mat( 1, 0 ) * mat( 2, 1 );
237 
238  result( 3, 0 ) =
239  mat( 1, 0 ) * mat( 2, 2 ) * mat( 3, 1 ) +
240  mat( 1, 1 ) * mat( 2, 0 ) * mat( 3, 2 ) +
241  mat( 1, 2 ) * mat( 2, 1 ) * mat( 3, 0 ) -
242  mat( 1, 0 ) * mat( 2, 1 ) * mat( 3, 2 ) -
243  mat( 1, 1 ) * mat( 2, 2 ) * mat( 3, 0 ) -
244  mat( 1, 2 ) * mat( 2, 0 ) * mat( 3, 1 );
245 
246  result( 3, 1 ) =
247  mat( 0, 0 ) * mat( 2, 1 ) * mat( 3, 2 ) +
248  mat( 0, 1 ) * mat( 2, 2 ) * mat( 3, 0 ) +
249  mat( 0, 2 ) * mat( 2, 0 ) * mat( 3, 1 ) -
250  mat( 0, 0 ) * mat( 2, 2 ) * mat( 3, 1 ) -
251  mat( 0, 1 ) * mat( 2, 0 ) * mat( 3, 2 ) -
252  mat( 0, 2 ) * mat( 2, 1 ) * mat( 3, 0 );
253 
254  result( 3, 2 ) =
255  mat( 0, 0 ) * mat( 1, 2 ) * mat( 3, 1 ) +
256  mat( 0, 1 ) * mat( 1, 0 ) * mat( 3, 2 ) +
257  mat( 0, 2 ) * mat( 1, 1 ) * mat( 3, 0 ) -
258  mat( 0, 0 ) * mat( 1, 1 ) * mat( 3, 2 ) -
259  mat( 0, 1 ) * mat( 1, 2 ) * mat( 3, 0 ) -
260  mat( 0, 2 ) * mat( 1, 0 ) * mat( 3, 1 );
261 
262  result( 3, 3 ) =
263  mat( 0, 0 ) * mat( 1, 1 ) * mat( 2, 2 ) +
264  mat( 0, 1 ) * mat( 1, 2 ) * mat( 2, 0 ) +
265  mat( 0, 2 ) * mat( 1, 0 ) * mat( 2, 1 ) -
266  mat( 0, 0 ) * mat( 1, 2 ) * mat( 2, 1 ) -
267  mat( 0, 1 ) * mat( 1, 0 ) * mat( 2, 2 ) -
268  mat( 0, 2 ) * mat( 1, 1 ) * mat( 2, 0 );
269 
270  WAssert( det != 0, "Determinat is zero. This matrix can not be inverted." );
271 
272  double detInv = 1. / det;
273  for( size_t r = 0; r < 4; ++r )
274  {
275  for( size_t c = 0; c < 4; ++c )
276  {
277  result( c, r ) *= detInv;
278  }
279  }
280 
281  return result;
282 }
283 
284 bool linearIndependent( const WVector3d& u, const WVector3d& v )
285 {
286  WVector3d cp = cross( u, v );
287  if( std::fabs( cp[0] ) < wlimits::DBL_EPS && std::fabs( cp[1] ) < wlimits::DBL_EPS && std::fabs( cp[2] ) < wlimits::DBL_EPS )
288  {
289  return false;
290  }
291  return true;
292 }
WMatrix::getNbRows
size_t getNbRows() const
Get number of rows.
Definition: WMatrix.h:375
WMatrix::getNbCols
size_t getNbCols() const
Get number of columns.
Definition: WMatrix.h:383
WPosition
This only is a 3d double vector.
wlimits::DBL_EPS
const double DBL_EPS
Smallest double such: 1.0 + DBL_EPS == 1.0 is still true.
Definition: WLimits.cpp:46
Generated by doxygen 1.9.1