Subversion Repositories gelsvn

Rev

Rev 379 | Go to most recent revision | Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
39 bj 1 
#include <iostream>
2 
#include <vector>
3 
#include "LapackFunc.h"
4 
 
5 
extern "C" {
6 
	extern void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info);
7 
	extern void dgetri_(const int *n, double *a, const int *lda, int *ipiv,double *work, int *lwork, int *info);
8 
	extern int dgesvd_(const char *jobu, const char *jobvt, const int *m, const int *n, double *a, const int *lda, double *s,
9 
		double *u, const int *ldu, double *vt, const int *ldvt, double *work, const int *lwork, int *info);
10 
	extern void dgelss_(const int *, const int *, const int *, double *, const int *, double *, const int *, double *, double *, int *, double *, const int *, int *);
11 
	extern void dgesv_(const int *N, const int *NRHS, double *A, const int *LDA, int *IPIV, double *B, const int *LDB, int *INFO);
12 
	extern void dposv_(const char *UPLO, const int *N, const int *NRHS, double *A, const int *LDA, double *B, const int *LDB, int *INFO);
13 
	extern void dgeqrf_(const int *m, const int *n, double* A,const int* lda,double*tau,double *work, const int* lwork, int* info);
14 
	extern void dorgqr_(const int *m, const int *n, const int *k, double* A, const int *lda,const double *tau,double* work, const int *lwork, int *info);
15 
	extern void dgerqf_(const int *m, const int *n, double* A,const int* lda,double*tau,double *work, const int* lwork, int* info);
16 
	extern void dorgrq_(const int *m, const int *n, const int *k, double* A, const int *lda,const double *tau,double* work, const int *lwork, int *info);
17 
 
18 
	int dsysv_(char *uplo, int *n, int *nrhs, double *a, int *lda, int *ipiv, double *b, int *ldb, double *work, int *lwork, int *info);
19 
}
20 
 
21 
namespace{
22 
 
23 
	template <class T>
24 
		inline T MIN(const T& a,const T& b)
25 
	{
26 
		return a<b?a:b;
27 
	}
28 
 
29 
	template <class T>
30 
		inline T MAX(const T& a,const T& b)
31 
	{
32 
		return a>b?a:b;
33 
	}
34 
 
35 
}
36 
 
37 
namespace LinAlg
38 
{
39 
 
40 
void SVD(const CMatrix& A,
41 
		 CMatrix& U,
42 
		 CVector& s,
43 
		 CMatrix& V)
44 
{
45 
	//Notice, that the Lapack/Fortran CMatrix representation is the transposed version of the C/C++
46 
	CMatrix a;
47 
	A.Transposed(a);
48 
 
49 
	int nRows=A.Rows();
50 
	int nCols=A.Cols();
51 
	int info;
52 
	int lda = MAX(nRows,1);
53 
	int lwork= 6*MIN(nRows,nCols) + nRows + nCols;
54 
 
55 
	s.Resize(MIN(nRows,nCols));
56 
	double *work = new double[lwork];
57 
 
58 
	char jobu='A';
59 
	int ldu=nRows;
60 
	U.Resize(nRows,nRows);
61 
 
62 
	char jobvt='A';
63 
	int ldvt=nCols;
64 
	V.Resize(nCols,nCols);
65 
 
66 
 
67 
	dgesvd_ (&jobu, &jobvt, &nRows, &nCols, a[0], &lda, &(s[0]), U[0], &ldu, V[0], &ldvt,work, &lwork, &info);
68 
 
69 
	assert(info==0);
70 
 
71 
	delete work;
72 
	U.Transpose();
73 
}
74 
 
75 
void SVD(const CMatrix& A,
76 
		 CMatrix& U,
77 
		 CMatrix& S,
78 
		 CMatrix& V)
79 
{
80 
	CVector s;
81 
	SVD(A,U,s,V);
82 
	S.Resize(A.Rows(),A.Cols());
83 
	S=0;
84 
	for(int cS=MIN(A.Rows(),A.Cols())-1;cS>=0;cS--)
85 
	{
86 
		S[cS][cS]=s[cS];
87 
	}
88 
}
89 
 
90 
CVector SVD(const CMatrix& A)
91 
{
92 
	CMatrix a;
93 
	A.Transposed(a);
94 
 
95 
	int nRows=A.Rows();
96 
	int nCols=A.Cols();
97 
	int info;
98 
	int lda = MAX(nRows,1);
99 
	int lwork= 6*MIN(nRows,nCols) + nRows + nCols;
100 
 
101 
	CVector s(MIN(nRows,nCols));
102 
	double *work = new double[lwork];
103 
 
104 
	char jobu='N';
105 
	int ldu=1;
106 
 
107 
	char jobvt='N';
108 
	int ldvt=1;
109 
 
110 
	dgesvd_ (&jobu, &jobvt, &nRows, &nCols, a[0], &lda, &(s[0]), NULL, &ldu, NULL, &ldvt,work, &lwork, &info);
111 
 
112 
	assert(info==0);
113 
 
114 
	delete work;
115 
 
116 
	return s;
117 
}
118 
 
119 
 
120 
void LinearSolve(const CMatrix& A,
121 
				 const CVector&b,
122 
				 CVector& x)
123 
{
124 
	assert(A.Rows()==b.Length());
125 
	assert(A.Rows()==A.Cols());
126 
 
127 
	CMatrix a;
128 
	A.Transposed(a);
129 
	x=b;
130 
	int nRows=A.Rows();
131 
	int nrhs=1;	//only one CVector, change here to make b a CMatrix.
132 
	int info;
133 
	int *ipiv =new int[nRows];
134 
 
135 
	dgesv_(&nRows, &nrhs, a[0], &nRows, ipiv, &(x[0]), &nRows, &info);
136 
 
137 
	assert(info==0);
138 
	delete [] ipiv;
139 
}
140 
 
141 
CVector LinearSolve(const CMatrix& A,
142 
				   const CVector&b)
143 
{
144 
	CVector x;
145 
	LinearSolve(A,b,x);
146 
	return x;
147 
}
148 
 
149 
 
150 
void LinearSolveSPD(const CMatrix& A,
151 
					const CVector&b,
152 
					CVector& x)
153 
{
154 
	assert(A.Rows()==b.Length());
155 
	assert(A.Rows()==A.Cols());
156 
 
157 
	CMatrix a(A);
158 
	x=b;
159 
 
160 
	char uplo='U';
161 
	int nRows=A.Rows();
162 
	int info;
163 
	int nrhs=1;
164 
 
165 
	dposv_(&uplo, &nRows, &nrhs, a[0], &nRows, &(x[0]), &nRows, &info);
166 
 
167 
	assert(info==0);
168 
}
169 
 
170 
void LinearSolveSym(const CMatrix& A,
171 
										const CVector&b,
172 
										CVector& x)
173 
{
174 
	assert(A.Rows()==b.Length());
175 
	assert(A.Rows()==A.Cols());
176 
 
177 
	CMatrix a(A);
178 
	x=b;
179 
 
180 
	char uplo='U';
181 
	int nRows=A.Rows();
182 
	int nCols=A.Cols();
183 
	int info;
184 
	int nrhs=1;
185 
 
186 
	int lwork= 10*6*MIN(nRows,nCols) + nRows + nCols;
187 
	double *work = new double[lwork];
188 
 
189 
	std::vector<int> ipiv(A.Rows());
190 
	dsysv_(&uplo, &nRows, &nrhs, a[0], &nRows, &ipiv[0], &(x[0]), &nRows,
191 
				 work, &lwork, &info);
192 
	delete work;
193 
	assert(info==0);
194 
}
195 
 
196 
 
197 
CVector LinearSolveSPD(const CMatrix& A,const CVector&b)
198 
{
199 
	CVector x;
200 
	LinearSolveSPD(A,b,x);
201 
	return x;
202 
}
203 
 
204 
void LinearLSSolve(const CMatrix& A,
205 
				   const CVector& b,
206 
				   CVector& x)
207 
{
208 
	assert(A.Rows()==b.Length());
209 
 
210 
	int nRows=A.Rows();
211 
	int nCols=A.Cols();
212 
	CMatrix a;
213 
	A.Transposed(a);
214 
 
215 
	int ldb=MAX(nRows,nCols);
216 
	double* BX=new double[ldb];
217 
	memcpy(BX,&(b[0]),nRows*sizeof(double));
218 
 
219 
	int nrhs=1;
220 
	double* s=new double[MIN(nRows,nCols)];
221 
	double rcond = -1.0; // using machine precision
222 
	int info,rank;
223 
	int lwork = 5*nRows*nCols + 1; // larger than necessary
224 
	double *work = new double[lwork];
225 
 
226 
	dgelss_(&nRows, &nCols, &nrhs, a[0], &nRows, BX, &ldb, s, &rcond, &rank, work, &lwork, &info);
227 
 
228 
	x.Resize(nCols);
229 
	memcpy(&(x[0]),BX,nCols*sizeof(double));
230 
 
231 
 
232 
	assert(info==0);
233 
 
234 
	delete s;
235 
	delete work;
236 
}
237 
 
238 
CVector LinearLSSolve(const CMatrix& A,
239 
					 const CVector& b)
240 
{
241 
	CVector x;
242 
	LinearLSSolve(A,b,x);
243 
	return x;
244 
}
245 
 
246 
void Invert(CMatrix& A)
247 
{
248 
	assert(A.Rows()==A.Cols());
249 
	int nRows=A.Rows();
250 
	int info;
251 
	int *ipiv = new int[nRows];
252 
	//Perform th LU factorization of A
253 
	dgetrf_ (&nRows, &nRows, A[0], &nRows, ipiv, &info);
254 
	if (info != 0) { //An error occured
255 
		delete [] ipiv;
256 
		assert(info==0);	//info will be < 0 if A is rank deficient.
257 
		return;
258 
	}
259 
 
260 
	//Calculate the inverse
261 
	int lwork = nRows * 5;
262 
	double *work = new double[lwork];
263 
	dgetri_ (&nRows, A[0], &nRows, ipiv, work, &lwork, &info);
264 
	delete [] ipiv;
265 
	delete [] work;
266 
	assert(info==0);
267 
}
268 
 
269 
void Inverted(const CMatrix& A,
270 
			  CMatrix& InvA)
271 
{
272 
	InvA=A;
273 
	Invert(InvA);
274 
}
275 
 
276 
CMatrix Inverted(const CMatrix& A)
277 
{
278 
	CMatrix InvA(A);
279 
	Invert(InvA);
280 
	return InvA;
281 
}
282 
 
283 
void QRfact(const CMatrix& A,CMatrix& Q, CMatrix& R)
284 
{
285 
 
286 
 
287 
	int nRows=A.Rows();
288 
	int nCols=A.Cols();
289 
	assert(A.Rows()>0 && A.Cols()>0); //herby lda=nRows.
290 
	int lwork= 6*nCols;
291 
	int info;
292 
	int k=MIN(nRows,nCols);
293 
 
294 
 
295 
	A.Transposed(R);
296 
 
297 
	double *work = new double[lwork];
298 
 
299 
 
300 
	CVector TAU(k);
301 
 
302 
	dgeqrf_(&nRows,&nCols,R[0],&nRows,&TAU[0],work,&lwork,&info);
303 
	assert(info==0);
304 
 
305 
 
306 
	Q=R;
307 
 
308 
	nCols=MIN(nCols,nRows);
309 
 
310 
	dorgqr_(&nRows,&nCols,&k,Q[0],&nRows,&TAU[0],work,&lwork,&info);
311 
	assert(info==0);
312 
 
313 
 
314 
	delete work;
315 
 
316 
	R.Transpose();
317 
	Q.Transpose();
318 
 
319 
	//Set the zeros of R
320 
	double * pRow;
321 
	int ColStop;
322 
	for(int cRow=1;cRow<R.Rows();cRow++)
323 
	{
324 
		pRow=R[cRow];
325 
		ColStop=MIN(cRow,R.Cols());
326 
		for(int cCol=0;cCol<ColStop;cCol++)
327 
		{
328 
			pRow[cCol]=0;
329 
		}
330 
	}
331 
 
332 
}
333 
 
334 
 
335 
void RQfact(const CMatrix& A,CMatrix& R, CMatrix& Q)
336 
{
337 
	int nRows=A.Rows();
338 
	int nCols=A.Cols();
339 
	assert(A.Rows()>0 && A.Cols()>0); //herby lda=nRows.
340 
	int lwork= 6*nCols;
341 
	int info;
342 
	int k=MIN(nRows,nCols);
343 
 
344 
 
345 
	A.Transposed(R);
346 
 
347 
	double *work = new double[lwork];
348 
 
349 
 
350 
	CVector TAU(k);
351 
 
352 
	dgerqf_(&nRows,&nCols,R[0],&nRows,&TAU[0],work,&lwork,&info);
353 
	assert(info==0);
354 
 
355 
	Q=R;
356 
 
357 
	nCols=MAX(nCols,nRows);
358 
 
359 
	dorgrq_(&nRows,&nCols,&k,Q[0],&nRows,&TAU[0],work,&lwork,&info);
360 
	assert(info==0);
361 
 
362 
 
363 
	delete work;
364 
 
365 
	R.Transpose();
366 
	Q.Transpose();
367 
 
368 
	//Set the zeros of R
369 
	double * pRow;
370 
	int ColStop;
371 
	for(int cRow=1;cRow<R.Rows();cRow++)
372 
	{
373 
		pRow=R[cRow];
374 
		ColStop=MIN(cRow,R.Cols());
375 
		for(int cCol=0;cCol<ColStop;cCol++)
376 
		{
377 
			pRow[cCol]=0;
378 
		}
379 
	}
380 
 
381 
}
382 
 
383 
 
384 
}