Rev Author Line No. Line
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