178 |
kaklik |
1 |
#ifndef LINALG_TCC |
|
|
2 |
#define LINALG_TCC |
|
|
3 |
|
|
|
4 |
#include <algorithm> |
|
|
5 |
#include <boost/shared_ptr.hpp> |
|
|
6 |
#include <boost/static_assert.hpp> |
|
|
7 |
#include <boost/type_traits.hpp> |
|
|
8 |
#include <cmath> |
|
|
9 |
extern "C" { |
|
|
10 |
#include <f2c.h> |
|
|
11 |
#include "clapack.h" |
|
|
12 |
} |
|
|
13 |
|
|
|
14 |
// Problem with f2c defining macros. |
|
|
15 |
#ifdef abs |
|
|
16 |
#undef abs |
|
|
17 |
#endif |
|
|
18 |
#ifdef dabs |
|
|
19 |
#undef dabs |
|
|
20 |
#endif |
|
|
21 |
#ifdef min |
|
|
22 |
#undef min |
|
|
23 |
#endif |
|
|
24 |
#ifdef max |
|
|
25 |
#undef max |
|
|
26 |
#endif |
|
|
27 |
#ifdef dmin |
|
|
28 |
#undef dmin |
|
|
29 |
#endif |
|
|
30 |
#ifdef dmax |
|
|
31 |
#undef dmax |
|
|
32 |
#endif |
|
|
33 |
#ifdef bit_test |
|
|
34 |
#undef bit_test |
|
|
35 |
#endif |
|
|
36 |
#ifdef bit_clear |
|
|
37 |
#undef bit_clear |
|
|
38 |
#endif |
|
|
39 |
#ifdef bit_set |
|
|
40 |
#undef bit_set |
|
|
41 |
#endif |
|
|
42 |
|
|
|
43 |
namespace mimas { |
|
|
44 |
|
|
|
45 |
template< typename T > |
|
|
46 |
boost::numeric::ublas::vector< T > crossProduct |
|
|
47 |
( boost::numeric::ublas::vector< T > &a, |
|
|
48 |
boost::numeric::ublas::vector< T > &b ) |
|
|
49 |
{ |
|
|
50 |
assert( a.size() == 3 && b.size() == 3 ); |
|
|
51 |
boost::numeric::ublas::vector< T > retVal( 3 ); |
|
|
52 |
retVal(0) = a(1) * b(2) - a(2) * b(1); |
|
|
53 |
retVal(1) = a(2) * b(0) - a(0) * b(2); |
|
|
54 |
retVal(2) = a(0) * b(1) - a(1) * b(0); |
|
|
55 |
return retVal; |
|
|
56 |
} |
|
|
57 |
|
|
|
58 |
template< typename T > |
|
|
59 |
boost::numeric::ublas::vector< T > rodrigues |
|
|
60 |
( boost::numeric::ublas::vector< T > const &u, |
|
|
61 |
boost::numeric::ublas::vector< T > const &v, |
|
|
62 |
double angle) |
|
|
63 |
{ |
|
|
64 |
boost::numeric::ublas::matrix< T > m(3,3); |
|
|
65 |
boost::numeric::ublas::vector< T > w; |
|
|
66 |
|
|
|
67 |
m(0,0) = cos(angle) + u(0) * u(0) * (1 - cos(angle)); |
|
|
68 |
m(0,1) = u(0) * u(1) * (1 - cos(angle)) - u(2) * sin(angle); |
|
|
69 |
m(0,2) = u(1) * sin(angle) + u(0) * u(2) * (1 - cos(angle)); |
|
|
70 |
m(1,0) = u(2) * sin(angle) + u(0) * u(1) * (1 - cos(angle)); |
|
|
71 |
m(1,1) = cos(angle) + u(1) * u(1) * (1 - cos(angle)); |
|
|
72 |
m(1,2) = - u(0) * sin(angle) + u(1) * u(2) * (1 - cos(angle)); |
|
|
73 |
m(2,0) = - u(1) * sin(angle) + u(0) * u(2) * (1 - cos(angle)); |
|
|
74 |
m(2,1) = u(0) * sin(angle) + u(1) * u(2) * (1 - cos(angle)); |
|
|
75 |
m(2,2) = cos(angle) + u(2) * u(2) * (1 - cos(angle)); |
|
|
76 |
|
|
|
77 |
w = boost::numeric::ublas::prod(m, v); |
|
|
78 |
|
|
|
79 |
return w; |
|
|
80 |
} |
|
|
81 |
|
|
|
82 |
|
|
|
83 |
template< typename T > |
|
|
84 |
double determinant (boost::numeric::ublas::matrix< T > const &M) { |
|
|
85 |
// create a working copy of the input |
|
|
86 |
boost::numeric::ublas::matrix< T > mLu(M); |
|
|
87 |
boost::numeric::ublas::permutation_matrix< std::size_t > pivots(M.size1()); |
|
|
88 |
|
|
|
89 |
// use uBLAS LU decomposition |
|
|
90 |
boost::numeric::ublas::lu_factorize(mLu, pivots); |
|
|
91 |
|
|
|
92 |
double det = 1.0; |
|
|
93 |
|
|
|
94 |
// compute the determinant |
|
|
95 |
for (std::size_t i=0; i < pivots.size(); ++i) { |
|
|
96 |
if (pivots(i) != i) |
|
|
97 |
det *= -1.0; |
|
|
98 |
det *= mLu(i,i); |
|
|
99 |
} |
|
|
100 |
return det; |
|
|
101 |
} |
|
|
102 |
|
|
|
103 |
|
|
|
104 |
// Helping function-object for gglse. |
|
|
105 |
template< typename T > |
|
|
106 |
struct gglse_ |
|
|
107 |
{ |
|
|
108 |
int operator()( integer *m, integer *n, integer *p, T *a, integer *lda, |
|
|
109 |
T *b, integer *ldb, T *c__, T *d__, T *x, T *work, |
|
|
110 |
integer *lwork, integer *info); |
|
|
111 |
}; |
|
|
112 |
|
|
|
113 |
// gglse lapack invocation. |
|
|
114 |
template< typename T > |
|
|
115 |
typename gglse_t< T >::Vector gglse_t< T >::operator() |
|
|
116 |
( const Matrix &_A, const Matrix &_B, |
|
|
117 |
const Vector &_c, const Vector &_d ) const |
|
|
118 |
{ |
|
|
119 |
BOOST_STATIC_ASSERT( boost::is_float< T >::value ); |
|
|
120 |
Matrix A( _A ), B( _B ); |
|
|
121 |
Vector c( _c ), d( _d ); |
|
|
122 |
assert( A.size1() == c.size() ); |
|
|
123 |
assert( B.size1() == d.size() ); |
|
|
124 |
assert( A.size2() == B.size2() ); |
|
|
125 |
Vector x( A.size2() ); |
|
|
126 |
integer |
|
|
127 |
M = A.size1(), |
|
|
128 |
N = A.size2(), |
|
|
129 |
P = B.size1(), |
|
|
130 |
LDA = A.size1(), |
|
|
131 |
LDB = B.size1(), |
|
|
132 |
LWORK = -1, |
|
|
133 |
INFO = 0; |
|
|
134 |
T DLWORK; |
|
|
135 |
gglse_< T > gglse_; |
|
|
136 |
gglse_( &M, &N, &P, A.data().begin(), &LDA, B.data().begin(), |
|
|
137 |
&LDB, c.data().begin(), d.data().begin(), |
|
|
138 |
x.data().begin(), &DLWORK, &LWORK, &INFO ); |
|
|
139 |
assert( INFO == 0 ); |
|
|
140 |
LWORK = (integer)DLWORK; |
|
|
141 |
T WORK[LWORK]; |
|
|
142 |
gglse_( &M, &N, &P, A.data().begin(), &LDA, B.data().begin(), |
|
|
143 |
&LDB, c.data().begin(), d.data().begin(), |
|
|
144 |
x.data().begin(), &WORK[0], &LWORK, &INFO ); |
|
|
145 |
assert( INFO == 0 ); |
|
|
146 |
return x; |
|
|
147 |
} |
|
|
148 |
|
|
|
149 |
// Helping function objects for inv. |
|
|
150 |
template< typename T > |
|
|
151 |
struct getrf_ |
|
|
152 |
{ |
|
|
153 |
int operator()( integer *m, integer *n, T *a, integer *lda, integer *ipiv, |
|
|
154 |
integer *info ) const; |
|
|
155 |
}; |
|
|
156 |
|
|
|
157 |
template< typename T > |
|
|
158 |
struct getri_ |
|
|
159 |
{ |
|
|
160 |
int operator()( integer *n, T *a, integer *lda, integer *ipiv, T *work, |
|
|
161 |
integer *lwork, integer *info ) const; |
|
|
162 |
}; |
|
|
163 |
|
|
|
164 |
// inv lapack invocation. |
|
|
165 |
template< typename T > |
|
|
166 |
typename inv_t< T >::Matrix inv_t< T >::operator()( const Matrix &A ) const |
|
|
167 |
{ |
|
|
168 |
BOOST_STATIC_ASSERT( boost::is_float< T >::value ); |
|
|
169 |
assert( A.size1() == A.size2() ); |
|
|
170 |
Matrix AInv( A ); |
|
|
171 |
integer |
|
|
172 |
N = A.size1(), |
|
|
173 |
LDA = A.size1(), |
|
|
174 |
LWORK = -1, |
|
|
175 |
IPIV[N], |
|
|
176 |
INFO = 0; |
|
|
177 |
getrf_< T >()( &N, &N, AInv.data().begin(), &LDA, &IPIV[0], &INFO ); |
|
|
178 |
assert( INFO == 0 ); |
|
|
179 |
T DLWORK; |
|
|
180 |
getri_< T > getri_; |
|
|
181 |
getri_( &N, AInv.data().begin(), &LDA, &IPIV[0], &DLWORK, &LWORK, |
|
|
182 |
&INFO ); |
|
|
183 |
LWORK = (integer)DLWORK; |
|
|
184 |
T WORK[LWORK]; |
|
|
185 |
getri_( &N, AInv.data().begin(), &LDA, &IPIV[0], &WORK[0], &LWORK, |
|
|
186 |
&INFO ); |
|
|
187 |
return AInv; |
|
|
188 |
}; |
|
|
189 |
|
|
|
190 |
// Helping function object for gels |
|
|
191 |
template< typename T > |
|
|
192 |
struct gels_ |
|
|
193 |
{ |
|
|
194 |
int operator()( char *trans, integer *m, integer *n, integer *nrhs, |
|
|
195 |
T *a, integer *lda, T *b, integer *ldb, T *work, |
|
|
196 |
integer *lwork, integer *info ) const; |
|
|
197 |
}; |
|
|
198 |
|
|
|
199 |
// gels lapack invocation |
|
|
200 |
template< typename T > |
|
|
201 |
typename gels_t<T>::Vector gels_t< T >::operator() |
|
|
202 |
( const Matrix &_A, const Vector &_b ) const |
|
|
203 |
{ |
|
|
204 |
BOOST_STATIC_ASSERT( boost::is_float< T >::value ); |
|
|
205 |
Matrix A( _A ); |
|
|
206 |
Vector b( _b ); |
|
|
207 |
assert( A.size1() >= A.size2() ); |
|
|
208 |
assert( A.size1() == b.size() ); |
|
|
209 |
char TRANS = 'N'; |
|
|
210 |
integer |
|
|
211 |
M = A.size1(), |
|
|
212 |
N = A.size2(), |
|
|
213 |
NHRS = 1, |
|
|
214 |
LDA = A.size1(), |
|
|
215 |
LDB = b.size(), |
|
|
216 |
LWORK = -1, |
|
|
217 |
INFO = 0; |
|
|
218 |
T DLWORK; |
|
|
219 |
gels_< T > gels_; |
|
|
220 |
gels_( &TRANS, &M, &N, &NHRS, A.data().begin(), &LDA, |
|
|
221 |
b.data().begin(), &LDB, &DLWORK, &LWORK, &INFO ); |
|
|
222 |
assert( INFO == 0 ); |
|
|
223 |
LWORK = (integer)DLWORK; |
|
|
224 |
T WORK[LWORK]; |
|
|
225 |
gels_( &TRANS, &M, &N, &NHRS, A.data().begin(), &LDA, |
|
|
226 |
b.data().begin(), &LDB, &WORK[0], &LWORK, &INFO ); |
|
|
227 |
assert( INFO == 0 ); |
|
|
228 |
boost::numeric::ublas::vector< T > x( A.size2() ); |
|
|
229 |
std::copy( b.data().begin(), b.data().begin() + A.size2(), |
|
|
230 |
x.data().begin() ); |
|
|
231 |
return x; |
|
|
232 |
} |
|
|
233 |
|
|
|
234 |
// Helping function object for gesvd |
|
|
235 |
template< typename T > |
|
|
236 |
struct gesvd_ |
|
|
237 |
{ |
|
|
238 |
int operator()( char *jobu, char *jobvt, integer *m, integer *n, |
|
|
239 |
T *a, integer *lda, T *s, T *u, integer *ldu, T *vt, |
|
|
240 |
integer *ldvt, T *work, integer *lwork, integer *info) const; |
|
|
241 |
}; |
|
|
242 |
|
|
|
243 |
// gesvd lapack invocation. |
|
|
244 |
template< typename T > |
|
|
245 |
typename gesvd_t< T >::BandedMatrix gesvd_t< T >::operator() |
|
|
246 |
( const Matrix &_A, Matrix *_U, Matrix *_Vt ) throw (mimasexception) |
|
|
247 |
{ |
|
|
248 |
BOOST_STATIC_ASSERT( boost::is_float< T >::value ); |
|
|
249 |
char JOBU; |
|
|
250 |
char JOBVT; |
|
|
251 |
Matrix A( _A ); |
|
|
252 |
T *U, *Vt; |
|
|
253 |
if ( _U != NULL ) { |
|
|
254 |
JOBU = 'A'; |
|
|
255 |
_U->resize( A.size1(), A.size1() ); |
|
|
256 |
U = _U->data().begin(); |
|
|
257 |
} else { |
|
|
258 |
JOBU = 'N'; |
|
|
259 |
U = NULL; |
|
|
260 |
}; |
|
|
261 |
if ( _Vt != NULL ) { |
|
|
262 |
JOBVT = 'A'; |
|
|
263 |
_Vt->resize( A.size2(), A.size2() ); |
|
|
264 |
Vt = _Vt->data().begin(); |
|
|
265 |
} else { |
|
|
266 |
JOBVT = 'N'; |
|
|
267 |
Vt = NULL; |
|
|
268 |
}; |
|
|
269 |
|
|
|
270 |
BandedMatrix S( A.size1(), A.size2(), 0, 0 ); |
|
|
271 |
integer |
|
|
272 |
M = A.size1(), |
|
|
273 |
N = A.size2(), |
|
|
274 |
LDA = A.size1(), |
|
|
275 |
LDU = A.size1(), |
|
|
276 |
LDVT = A.size2(), |
|
|
277 |
LWORK = -1, |
|
|
278 |
INFO = 0;// INFO may be left unmodified by dgesvd/sgesvd. |
|
|
279 |
gesvd_< T > gesvd_; |
|
|
280 |
T DLWORK; |
|
|
281 |
gesvd_( &JOBU, &JOBVT, &M, &N, A.data().begin(), &LDA, S.data().begin(), |
|
|
282 |
U, &LDU, Vt, &LDVT, &DLWORK, &LWORK, &INFO ); |
|
|
283 |
assert( INFO == 0 ); |
|
|
284 |
LWORK = (integer)DLWORK; |
|
|
285 |
T WORK[LWORK]; |
|
|
286 |
gesvd_( &JOBU, &JOBVT, &M, &N, A.data().begin(), &LDA, S.data().begin(), |
|
|
287 |
U, &LDU, Vt, &LDVT, &WORK[0], &LWORK, &INFO ); |
|
|
288 |
assert( INFO >= 0 ); |
|
|
289 |
MMERROR( INFO <= 0, mimasexception, , |
|
|
290 |
"gesvd: singular value decomposition didn't converge." ); |
|
|
291 |
return S; |
|
|
292 |
} |
|
|
293 |
|
|
|
294 |
// Helping function object for gesdd |
|
|
295 |
template< typename T > |
|
|
296 |
struct gesdd_ |
|
|
297 |
{ |
|
|
298 |
int operator()( char *jobz, integer *m, integer *n, T *a, integer *lda, |
|
|
299 |
T *s, T *u, integer *ldu, T *vt, integer *ldvt, T *work, |
|
|
300 |
integer *lwork, integer *iwork, integer *info ); |
|
|
301 |
}; |
|
|
302 |
|
|
|
303 |
// gesdd lapack invocation. |
|
|
304 |
template< typename T > |
|
|
305 |
typename gesdd_t< T >::BandedMatrix gesdd_t< T >::operator() |
|
|
306 |
( const Matrix &_A, Matrix *_U, Matrix *_Vt ) throw (mimasexception) |
|
|
307 |
{ |
|
|
308 |
BOOST_STATIC_ASSERT( boost::is_float< T >::value ); |
|
|
309 |
char JOBZ; |
|
|
310 |
Matrix A( _A ); |
|
|
311 |
assert( ( _U != NULL ) == ( _Vt != NULL ) ); |
|
|
312 |
T *U, *Vt; |
|
|
313 |
if ( _U != NULL ) { |
|
|
314 |
JOBZ = 'A'; |
|
|
315 |
_U->resize( A.size1(), A.size1() ); |
|
|
316 |
_Vt->resize( A.size2(), A.size2() ); |
|
|
317 |
U = _U->data().begin(); |
|
|
318 |
Vt = _Vt->data().begin(); |
|
|
319 |
} else { |
|
|
320 |
JOBZ = 'N'; |
|
|
321 |
U = NULL; |
|
|
322 |
Vt = NULL; |
|
|
323 |
}; |
|
|
324 |
|
|
|
325 |
BandedMatrix S( A.size1(), A.size2(), 0, 0 ); |
|
|
326 |
integer |
|
|
327 |
M = A.size1(), |
|
|
328 |
N = A.size2(), |
|
|
329 |
LDA = A.size1(), |
|
|
330 |
LDU = A.size1(), |
|
|
331 |
LDVT = A.size2(), |
|
|
332 |
LWORK = -1, |
|
|
333 |
IWORK[ 8 * std::min( M, N ) ], |
|
|
334 |
INFO = 0;// INFO may be left unmodified by dgesdd/sgesdd. |
|
|
335 |
gesdd_< T > gesdd_; |
|
|
336 |
#if 0 |
|
|
337 |
T DLWORK; |
|
|
338 |
gesdd_( &JOBZ, &M, &N, A.data().begin(), &LDA, S.data().begin(), |
|
|
339 |
U, &LDU, Vt, &LDVT, &DLWORK, &LWORK, &IWORK[0], &INFO ); |
|
|
340 |
assert( INFO == 0 ); |
|
|
341 |
LWORK = (integer)DLWORK; |
|
|
342 |
#else |
|
|
343 |
// Mandrake's liblapack3-package doesn't contain recent bugfixes of lapack. |
|
|
344 |
// See http://netlib2.cs.utk.edu/lapack/release_notes.html for bug in sgesdd. |
|
|
345 |
if ( JOBZ == 'N' ) |
|
|
346 |
LWORK = 3*std::min(M,N) + std::max(std::max(M,N),6*std::min(M,N)); |
|
|
347 |
else { |
|
|
348 |
assert( JOBZ == 'A' ); |
|
|
349 |
LWORK = 3*std::min(M,N)*std::min(M,N) + std::max(std::max(M,N),4*std::min(M,N)*std::min(M,N)+4*std::min(M,N)); |
|
|
350 |
}; |
|
|
351 |
#endif |
|
|
352 |
T WORK[LWORK]; |
|
|
353 |
gesdd_( &JOBZ, &M, &N, A.data().begin(), &LDA, S.data().begin(), |
|
|
354 |
U, &LDU, Vt, &LDVT, &WORK[0], &LWORK, &IWORK[0], &INFO ); |
|
|
355 |
assert( INFO >= 0 ); |
|
|
356 |
MMERROR( INFO <= 0, mimasexception, , |
|
|
357 |
"gesdd: singular value decomposition didn't converge." ); |
|
|
358 |
return S; |
|
|
359 |
} |
|
|
360 |
|
|
|
361 |
// Helping function for syev |
|
|
362 |
template< typename T > |
|
|
363 |
struct syev_ |
|
|
364 |
{ |
|
|
365 |
int operator()( char *jobz, char *uplo, integer *n, T *a, |
|
|
366 |
integer *lda, T *w, T *work, |
|
|
367 |
integer *lwork, integer *info) const; |
|
|
368 |
}; |
|
|
369 |
|
|
|
370 |
// syev lapack invocation. |
|
|
371 |
template< typename T > |
|
|
372 |
typename syev_t< T >::EigenValues syev_t< T >::operator() |
|
|
373 |
( const SymmetricMatrix &_A, Matrix *_E ) const throw (mimasexception) |
|
|
374 |
{ |
|
|
375 |
BOOST_STATIC_ASSERT( boost::is_float< T >::value ); |
|
|
376 |
char |
|
|
377 |
JOBZ = _E == NULL ? 'N' : 'V', |
|
|
378 |
UPLO = 'L'; |
|
|
379 |
|
|
|
380 |
boost::shared_ptr< Matrix > A; |
|
|
381 |
if ( _E == NULL ) { |
|
|
382 |
A = boost::shared_ptr< Matrix >( new Matrix( _A ) ); |
|
|
383 |
_E = A.get(); |
|
|
384 |
} else |
|
|
385 |
*_E = _A; |
|
|
386 |
|
|
|
387 |
EigenValues w( _E->size1() ); |
|
|
388 |
|
|
|
389 |
integer |
|
|
390 |
N = _E->size2(), |
|
|
391 |
LDA = _E->size1(), |
|
|
392 |
LWORK = -1, |
|
|
393 |
INFO = 0; |
|
|
394 |
T DLWORK; |
|
|
395 |
syev_< T > syev_; |
|
|
396 |
syev_( &JOBZ, &UPLO, &N, _E->data().begin(), &LDA, w.data().begin(), |
|
|
397 |
&DLWORK, &LWORK, &INFO ); |
|
|
398 |
assert( INFO == 0 ); |
|
|
399 |
LWORK = (integer)DLWORK; |
|
|
400 |
T WORK[LWORK]; |
|
|
401 |
syev_( &JOBZ, &UPLO, &N, _E->data().begin(), &LDA, w.data().begin(), |
|
|
402 |
&WORK[0], &LWORK, &INFO ); |
|
|
403 |
assert( INFO >= 0 ); |
|
|
404 |
MMERROR( INFO <= 0, mimasexception, , |
|
|
405 |
"syev: " << INFO << " off-diagonal elements of the " |
|
|
406 |
"intermediate eigentransform didn't converge." ); |
|
|
407 |
return w; |
|
|
408 |
} |
|
|
409 |
|
|
|
410 |
// Helping function object for pptrf |
|
|
411 |
template< typename T > |
|
|
412 |
struct pptrf_ |
|
|
413 |
{ |
|
|
414 |
int operator()( char *uplo, integer *n, T *ap, integer *info) const; |
|
|
415 |
}; |
|
|
416 |
|
|
|
417 |
// pptrf lapack invocation. |
|
|
418 |
template< typename T > |
|
|
419 |
typename pptrf_t< T >::TriangularMatrix pptrf_t< T >::operator() |
|
|
420 |
( const SymmetricMatrix &_A ) throw (mimasexception) |
|
|
421 |
{ |
|
|
422 |
BOOST_STATIC_ASSERT( boost::is_float< T >::value ); |
|
|
423 |
SymmetricMatrix A( _A ); |
|
|
424 |
char UPLO = 'U'; |
|
|
425 |
integer |
|
|
426 |
N = A.size1(), |
|
|
427 |
INFO = 0; |
|
|
428 |
pptrf_< T > pptrf_; |
|
|
429 |
|
|
|
430 |
pptrf_( &UPLO, &N, A.data().begin(), &INFO); |
|
|
431 |
MMERROR( INFO == 0, mimasexception, , |
|
|
432 |
"pptrf: " << INFO << " unable to compute the " |
|
|
433 |
"Cholesky decomposition." ); |
|
|
434 |
TriangularMatrix Tg( A ); |
|
|
435 |
return Tg; |
|
|
436 |
} |
|
|
437 |
|
|
|
438 |
}; |
|
|
439 |
|
|
|
440 |
#endif |