VMatrix.cpp 11.4 KB
Newer Older
1
2
3
4
5
6
7
/*
 * VMatrix.cpp
 *
 *  Created on: Feb 15, 2012
 *      Author: kleinwrt
 */

8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
/** \file
 *  VMatrix methods.
 *
 *  \author Claus Kleinwort, DESY, 2011 (Claus.Kleinwort@desy.de)
 *
 *  \copyright
 *  Copyright (c) 2011 - 2016 Deutsches Elektronen-Synchroton,
 *  Member of the Helmholtz Association, (DESY), HAMBURG, GERMANY \n\n
 *  This library is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU Library General Public License as
 *  published by the Free Software Foundation; either version 2 of the
 *  License, or (at your option) any later version. \n\n
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Library General Public License for more details. \n\n
 *  You should have received a copy of the GNU Library General Public
 *  License along with this program (see the file COPYING.LIB for more
 *  details); if not, write to the Free Software Foundation, Inc.,
 *  675 Mass Ave, Cambridge, MA 02139, USA.
 */

30
31
#include "VMatrix.h"

32
//! Namespace for the general broken lines package
Claus Kleinwort's avatar
Claus Kleinwort committed
33
namespace gbl {
34

35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
/*********** simple Matrix based on std::vector<double> **********/

VMatrix::VMatrix(const unsigned int nRows, const unsigned int nCols) :
		numRows(nRows), numCols(nCols), theVec(nRows * nCols) {
}

VMatrix::VMatrix(const VMatrix &aMatrix) :
		numRows(aMatrix.numRows), numCols(aMatrix.numCols), theVec(
				aMatrix.theVec) {

}

VMatrix::~VMatrix() {
}

/// Resize Matrix.
/**
 * \param [in] nRows Number of rows.
 * \param [in] nCols Number of columns.
 */
void VMatrix::resize(const unsigned int nRows, const unsigned int nCols) {
	numRows = nRows;
	numCols = nCols;
	theVec.resize(nRows * nCols);
}

/// Get transposed matrix.
/**
 * \return Transposed matrix.
 */
VMatrix VMatrix::transpose() const {
	VMatrix aResult(numCols, numRows);
67
68
	for (unsigned int i = 0; i < numRows; ++i) {
		for (unsigned int j = 0; j < numCols; ++j) {
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
			aResult(j, i) = theVec[numCols * i + j];
		}
	}
	return aResult;
}

/// Get number of rows.
/**
 * \return Number of rows.
 */
unsigned int VMatrix::getNumRows() const {
	return numRows;
}

/// Get number of columns.
/**
 * \return Number of columns.
 */
unsigned int VMatrix::getNumCols() const {
	return numCols;
}

/// Print matrix.
void VMatrix::print() const {
	std::cout << " VMatrix: " << numRows << "*" << numCols << std::endl;
94
95
	for (unsigned int i = 0; i < numRows; ++i) {
		for (unsigned int j = 0; j < numCols; ++j) {
96
97
98
99
100
101
102
103
104
105
106
107
108
109
			if (j % 5 == 0) {
				std::cout << std::endl << std::setw(4) << i << ","
						<< std::setw(4) << j << "-" << std::setw(4)
						<< std::min(j + 4, numCols) << ":";
			}
			std::cout << std::setw(13) << theVec[numCols * i + j];
		}
	}
	std::cout << std::endl << std::endl;
}

/// Multiplication Matrix*Vector.
VVector VMatrix::operator*(const VVector &aVector) const {
	VVector aResult(numRows);
110
	for (unsigned int i = 0; i < this->numRows; ++i) {
111
		double sum = 0.0;
112
		for (unsigned int j = 0; j < this->numCols; ++j) {
113
114
115
116
117
118
119
120
121
122
123
			sum += theVec[numCols * i + j] * aVector(j);
		}
		aResult(i) = sum;
	}
	return aResult;
}

/// Multiplication Matrix*Matrix.
VMatrix VMatrix::operator*(const VMatrix &aMatrix) const {

	VMatrix aResult(numRows, aMatrix.numCols);
124
125
	for (unsigned int i = 0; i < numRows; ++i) {
		for (unsigned int j = 0; j < aMatrix.numCols; ++j) {
126
			double sum = 0.0;
127
			for (unsigned int k = 0; k < numCols; ++k) {
128
129
130
131
132
133
134
135
136
137
138
				sum += theVec[numCols * i + k] * aMatrix(k, j);
			}
			aResult(i, j) = sum;
		}
	}
	return aResult;
}

/// Addition Matrix+Matrix.
VMatrix VMatrix::operator+(const VMatrix &aMatrix) const {
	VMatrix aResult(numRows, numCols);
139
140
	for (unsigned int i = 0; i < numRows; ++i) {
		for (unsigned int j = 0; j < numCols; ++j) {
141
142
143
144
145
146
			aResult(i, j) = theVec[numCols * i + j] + aMatrix(i, j);
		}
	}
	return aResult;
}

147
148
/// Assignment Matrix=Matrix.
VMatrix &VMatrix::operator=(const VMatrix &aMatrix) {
Claus Kleinwort's avatar
Claus Kleinwort committed
149
150
151
152
153
154
155
156
	if (this != &aMatrix) {   // Gracefully handle self assignment
		numRows = aMatrix.getNumRows();
		numCols = aMatrix.getNumCols();
		theVec.resize(numRows * numCols);
		for (unsigned int i = 0; i < numRows; ++i) {
			for (unsigned int j = 0; j < numCols; ++j) {
				theVec[numCols * i + j] = aMatrix(i, j);
			}
157
158
159
160
161
		}
	}
	return *this;
}

162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
/*********** simple symmetric Matrix based on std::vector<double> **********/

VSymMatrix::VSymMatrix(const unsigned int nRows) :
		numRows(nRows), theVec((nRows * nRows + nRows) / 2) {
}

VSymMatrix::~VSymMatrix() {
}

/// Resize symmetric matrix.
/**
 * \param [in] nRows Number of rows.
 */
void VSymMatrix::resize(const unsigned int nRows) {
	numRows = nRows;
	theVec.resize((nRows * nRows + nRows) / 2);
}

/// Get number of rows (= number of colums).
/**
 * \return Number of rows.
 */
unsigned int VSymMatrix::getNumRows() const {
	return numRows;
}

/// Print matrix.
void VSymMatrix::print() const {
	std::cout << " VSymMatrix: " << numRows << "*" << numRows << std::endl;
191
192
	for (unsigned int i = 0; i < numRows; ++i) {
		for (unsigned int j = 0; j <= i; ++j) {
193
194
195
196
197
198
199
200
201
202
203
204
205
206
			if (j % 5 == 0) {
				std::cout << std::endl << std::setw(4) << i << ","
						<< std::setw(4) << j << "-" << std::setw(4)
						<< std::min(j + 4, i) << ":";
			}
			std::cout << std::setw(13) << theVec[(i * i + i) / 2 + j];
		}
	}
	std::cout << std::endl << std::endl;
}

/// Subtraction SymMatrix-(sym)Matrix.
VSymMatrix VSymMatrix::operator-(const VMatrix &aMatrix) const {
	VSymMatrix aResult(numRows);
207
208
	for (unsigned int i = 0; i < numRows; ++i) {
		for (unsigned int j = 0; j <= i; ++j) {
209
210
211
212
213
214
215
216
217
			aResult(i, j) = theVec[(i * i + i) / 2 + j] - aMatrix(i, j);
		}
	}
	return aResult;
}

/// Multiplication SymMatrix*Vector.
VVector VSymMatrix::operator*(const VVector &aVector) const {
	VVector aResult(numRows);
218
	for (unsigned int i = 0; i < numRows; ++i) {
219
		aResult(i) = theVec[(i * i + i) / 2 + i] * aVector(i);
220
		for (unsigned int j = 0; j < i; ++j) {
221
222
223
224
225
226
227
228
229
230
231
			aResult(j) += theVec[(i * i + i) / 2 + j] * aVector(i);
			aResult(i) += theVec[(i * i + i) / 2 + j] * aVector(j);
		}
	}
	return aResult;
}

/// Multiplication SymMatrix*Matrix.
VMatrix VSymMatrix::operator*(const VMatrix &aMatrix) const {
	unsigned int nCol = aMatrix.getNumCols();
	VMatrix aResult(numRows, nCol);
232
233
	for (unsigned int l = 0; l < nCol; ++l) {
		for (unsigned int i = 0; i < numRows; ++i) {
234
			aResult(i, l) = theVec[(i * i + i) / 2 + i] * aMatrix(i, l);
235
			for (unsigned int j = 0; j < i; ++j) {
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
				aResult(j, l) += theVec[(i * i + i) / 2 + j] * aMatrix(i, l);
				aResult(i, l) += theVec[(i * i + i) / 2 + j] * aMatrix(j, l);
			}
		}
	}
	return aResult;
}

/*********** simple Vector based on std::vector<double> **********/

VVector::VVector(const unsigned int nRows) :
		numRows(nRows), theVec(nRows) {
}

VVector::VVector(const VVector &aVector) :
		numRows(aVector.numRows), theVec(aVector.theVec) {

}

VVector::~VVector() {
}

/// Resize vector.
/**
 * \param [in] nRows Number of rows.
 */
void VVector::resize(const unsigned int nRows) {
	numRows = nRows;
	theVec.resize(nRows);
}

/// Get part of vector.
/**
 * \param [in] len Length of part.
 * \param [in] start Offset of part.
 * \return Part of vector.
 */
VVector VVector::getVec(unsigned int len, unsigned int start) const {
	VVector aResult(len);
	std::memcpy(&aResult.theVec[0], &theVec[start], sizeof(double) * len);
	return aResult;
}

/// Put part of vector.
/**
 * \param [in] aVector Vector with part.
 * \param [in] start Offset of part.
 */
void VVector::putVec(const VVector &aVector, unsigned int start) {
	std::memcpy(&theVec[start], &aVector.theVec[0],
			sizeof(double) * aVector.numRows);
}

/// Get number of rows.
/**
 * \return Number of rows.
 */
unsigned int VVector::getNumRows() const {
	return numRows;
}

/// Print vector.
void VVector::print() const {
	std::cout << " VVector: " << numRows << std::endl;
300
	for (unsigned int i = 0; i < numRows; ++i) {
301
302
303
304
305
306
307
308
309
310
311
312
313

		if (i % 5 == 0) {
			std::cout << std::endl << std::setw(4) << i << "-" << std::setw(4)
					<< std::min(i + 4, numRows) << ":";
		}
		std::cout << std::setw(13) << theVec[i];
	}
	std::cout << std::endl << std::endl;
}

/// Subtraction Vector-Vector.
VVector VVector::operator-(const VVector &aVector) const {
	VVector aResult(numRows);
314
	for (unsigned int i = 0; i < numRows; ++i) {
315
316
317
318
319
		aResult(i) = theVec[i] - aVector(i);
	}
	return aResult;
}

320
321
/// Assignment Vector=Vector.
VVector &VVector::operator=(const VVector &aVector) {
Claus Kleinwort's avatar
Claus Kleinwort committed
322
323
324
325
326
327
	if (this != &aVector) {   // Gracefully handle self assignment
		numRows = aVector.getNumRows();
		theVec.resize(numRows);
		for (unsigned int i = 0; i < numRows; ++i) {
			theVec[i] = aVector(i);
		}
328
329
330
331
	}
	return *this;
}

332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
/*============================================================================
 from mpnum.F (MillePede-II by V. Blobel, Univ. Hamburg)
 ============================================================================*/
/// Matrix inversion.
/**
 *     Invert symmetric N-by-N matrix V in symmetric storage mode
 *               V(1) = V11, V(2) = V12, V(3) = V22, V(4) = V13, . . .
 *               replaced by inverse matrix
 *
 *     Method of solution is by elimination selecting the  pivot  on  the
 *     diagonal each stage. The rank of the matrix is returned in  NRANK.
 *     For NRANK ne N, all remaining  rows  and  cols  of  the  resulting
 *     matrix V are  set  to  zero.
 *  \exception 1 : matrix is singular.
 *  \return Rank of matrix.
 */
unsigned int VSymMatrix::invert() {

	const double eps = 1.0E-10;
	unsigned int aSize = numRows;
	std::vector<int> next(aSize);
	std::vector<double> diag(aSize);
	int nSize = aSize;

	int first = 1;
357
	for (int i = 1; i <= nSize; ++i) {
358
359
360
361
362
363
		next[i - 1] = i + 1; // set "next" pointer
		diag[i - 1] = fabs(theVec[(i * i + i) / 2 - 1]); // save abs of diagonal elements
	}
	next[aSize - 1] = -1; // end flag

	unsigned int nrank = 0;
364
	for (int i = 1; i <= nSize; ++i) { // start of loop
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
		int k = 0;
		double vkk = 0.0;

		int j = first;
		int previous = 0;
		int last = previous;
		// look for pivot
		while (j > 0) {
			int jj = (j * j + j) / 2 - 1;
			if (fabs(theVec[jj]) > std::max(fabs(vkk), eps * diag[j - 1])) {
				vkk = theVec[jj];
				k = j;
				last = previous;
			}
			previous = j;
			j = next[j - 1];
		}
		// pivot found
		if (k > 0) {
			int kk = (k * k + k) / 2 - 1;
			if (last <= 0) {
				first = next[k - 1];
			} else {
				next[last - 1] = next[k - 1];
			}
			next[k - 1] = 0; // index is used, reset
			nrank++; // increase rank and ...

			vkk = 1.0 / vkk;
			theVec[kk] = -vkk;
			int jk = kk - k;
			int jl = -1;
397
			for (int j = 1; j <= nSize; ++j) { // elimination
398
399
400
401
402
				if (j == k) {
					jk = kk;
					jl += j;
				} else {
					if (j < k) {
403
						++jk;
404
405
406
407
408
409
410
411
					} else {
						jk += j - 1;
					}

					double vjk = theVec[jk];
					theVec[jk] = vkk * vjk;
					int lk = kk - k;
					if (j >= k) {
412
413
414
						for (int l = 1; l <= k - 1; ++l) {
							++jl;
							++lk;
415
416
							theVec[jl] -= theVec[lk] * vjk;
						}
417
						++jl;
418
						lk = kk;
419
420
						for (int l = k + 1; l <= j; ++l) {
							++jl;
421
422
423
424
							lk += l - 1;
							theVec[jl] -= theVec[lk] * vjk;
						}
					} else {
425
426
427
						for (int l = 1; l <= j; ++l) {
							++jl;
							++lk;
428
429
430
431
432
433
							theVec[jl] -= theVec[lk] * vjk;
						}
					}
				}
			}
		} else {
434
			for (int k = 1; k <= nSize; ++k) {
435
436
				if (next[k - 1] >= 0) {
					int kk = (k * k - k) / 2 - 1;
437
					for (int j = 1; j <= nSize; ++j) {
438
439
440
441
442
443
444
445
446
						if (next[j - 1] >= 0) {
							theVec[kk + j] = 0.0; // clear matrix row/col
						}
					}
				}
			}
			throw 1; // singular
		}
	}
447
	for (int ij = 0; ij < (nSize * nSize + nSize) / 2; ++ij) {
448
449
450
451
		theVec[ij] = -theVec[ij]; // finally reverse sign of all matrix elements
	}
	return nrank;
}
452
}