1 package org.lcsim.recon.tracking.spacegeom;
2 import java.io.*;
3
4 class Matrix
5 {
6
7 protected int _row;
8 protected int _col;
9 protected double _a[][];
10 protected Matrix _eigenvalues;
11 protected Eigensystem _eigensystem;
12
13 public Matrix()
14 {
15 _row = 0;
16 _col = 0;
17 createData();
18 }
19
20 public Matrix(int i, int j)
21 {
22 _row = i;
23 _col = j;
24 createData();
25 }
26
27 public Matrix(Matrix matrix)
28 {
29 _row = matrix._row;
30 _col = matrix._col;
31 _eigensystem = matrix._eigensystem;
32 createData();
33 for(int i = 0; i < _row; i++)
34 {
35 for(int j = 0; j < _col; j++)
36 _a[i][j] = matrix._a[i][j];
37
38 }
39
40 }
41
42 protected void QL(Matrix matrix, Matrix matrix1, Matrix matrix2, boolean flag)
43 {
44 int k1 = _row;
45 for(int i = 1; i < k1; i++)
46 matrix2.set(i - 1, 0, matrix2.at(i, 0));
47
48 matrix2.set(k1 - 1, 0, 0.0D);
49 for(int l = 0; l < k1; l++)
50 {
51 int j1 = 0;
52 int i1;
53 do
54 {
55 for(i1 = l; i1 < k1 - 1; i1++)
56 {
57 double d2 = Math.abs(matrix1.at(i1, 0)) + Math.abs(matrix1.at(i1 + 1, 0));
58 if(Math.abs(matrix2.at(i1, 0)) + d2 == d2)
59 break;
60 }
61
62 if(i1 != l)
63 {
64 if(j1++ == 30)
65 {
66 System.err.println("Matrix::QL -> Too many iterations.");
67 System.exit(0);
68 }
69 double d5 = (matrix1.at(l + 1, 0) - matrix1.at(l, 0)) / (2D * matrix2.at(l, 0));
70 double d7 = pythagoras(d5, 1.0D);
71 d5 = (matrix1.at(i1, 0) - matrix1.at(l, 0)) + matrix2.at(l, 0) / (d5 + SIGN(d7, d5));
72 double d1;
73 double d8 = d1 = 1.0D;
74 double d6 = 0.0D;
75 int j;
76 for(j = i1 - 1; j >= l; j--)
77 {
78 double d3 = d8 * matrix2.at(j, 0);
79 double d = d1 * matrix2.at(j, 0);
80 matrix2.set(j + 1, 0, d7 = pythagoras(d3, d5));
81 if(d7 == 0.0D)
82 {
83 matrix1.set(j + 1, 0, matrix1.at(j + 1, 0) - d6);
84 matrix2.set(i1, 0, 0.0D);
85 break;
86 }
87 d8 = d3 / d7;
88 d1 = d5 / d7;
89 d5 = matrix1.at(j + 1, 0) - d6;
90 d7 = (matrix1.at(j, 0) - d5) * d8 + 2D * d1 * d;
91 matrix1.set(j + 1, 0, d5 + (d6 = d8 * d7));
92 d5 = d1 * d7 - d;
93 if(flag)
94 {
95 for(int k = 0; k < k1; k++)
96 {
97 double d4 = matrix.at(k, j + 1);
98 matrix.set(k, j + 1, d8 * matrix.at(k, j) + d1 * d4);
99 matrix.set(k, j, d1 * matrix.at(k, j) - d8 * d4);
100 }
101
102 }
103 }
104
105 if(d7 != 0.0D || j < l)
106 {
107 matrix1.set(l, 0, matrix1.at(l, 0) - d6);
108 matrix2.set(l, 0, d5);
109 matrix2.set(i1, 0, 0.0D);
110 }
111 }
112 } while(i1 != l);
113 }
114
115 }
116
117 protected double SIGN(double d, double d1)
118 {
119 if(d1 >= 0.0D)
120 return Math.abs(d);
121 else
122 return -Math.abs(d);
123 }
124
125
126 public double at(int i, int j)
127 {
128 return _a[i][j];
129 }
130
131 public int cols()
132 {
133 return _col;
134 }
135
136 protected void createData()
137 {
138 int i = _row + 1;
139 int j = _col + 1;
140 _a = new double[i][j];
141 }
142
143 public Eigensystem eigensystem()
144 {
145 if(!isSymmetric())
146 {
147 System.err.println("Matrix::eigensystem -> Matrix is not symmetric.");
148 System.exit(0);
149 }
150 int i = _row;
151 Matrix matrix = new Matrix(this);
152 Matrix matrix1 = new Matrix(i, 1);
153 Matrix matrix2 = new Matrix(i, 1);
154 householder(matrix, matrix1, matrix2, true);
155 QL(matrix, matrix1, matrix2, true);
156 if(_eigensystem == null)
157 _eigensystem = new Eigensystem();
158 Matrix matrix3 = new Matrix(matrix);
159 _eigensystem.setEigenvectors(matrix3);
160 Matrix matrix4 = new Matrix(matrix1);
161 _eigensystem.setEigenvalues(matrix4);
162 return _eigensystem;
163 }
164
165 public Matrix eigenvalues()
166 {
167 if(!isSymmetric())
168 {
169 System.err.println("Matrix::eigenvalues -> Matrix is not symmetric.");
170 System.exit(0);
171 }
172 int i = _row;
173 Matrix matrix = new Matrix(this);
174 Matrix matrix1 = new Matrix(i, 1);
175 Matrix matrix2 = new Matrix(i, 1);
176 householder(matrix, matrix1, matrix2, false);
177 QL(matrix, matrix1, matrix2, false);
178 if(_eigenvalues == null)
179 _eigenvalues = new Matrix(i, 1);
180 _eigenvalues = new Matrix(matrix1);
181 return _eigenvalues;
182 }
183
184
185 public Eigensystem getEigensystem()
186 {
187 return _eigensystem;
188 }
189
190 public Matrix getEigenvalues()
191 {
192 return _eigenvalues;
193 }
194
195 protected void householder(Matrix matrix, Matrix matrix1, Matrix matrix2, boolean flag)
196 {
197 int j3 = 0;
198 int k3 = _row;
199 for(int i = k3 - 1; i > 0; i--)
200 {
201 j3 = i - 1;
202 double d;
203 double d7 = d = 0.0D;
204 if(j3 > 0)
205 {
206 for(int k1 = 0; k1 <= j3; k1++)
207 d += Math.abs(matrix.at(i, k1));
208
209 if(d == 0.0D)
210 {
211 matrix2.set(i, 0, matrix.at(i, i));
212 }
213 else
214 {
215 for(int l1 = 0; l1 <= j3; l1++)
216 {
217 matrix.set(i, l1, matrix.at(i, l1) / d);
218 d7 += matrix.at(i, l1) * matrix.at(i, l1);
219 }
220
221 double d1 = matrix.at(i, j3);
222 double d3 = d1 < 0.0D ? Math.sqrt(d7) : -Math.sqrt(d7);
223 matrix2.set(i, 0, d * d3);
224 d7 -= d1 * d3;
225 matrix.set(i, j3, d1 - d3);
226 d1 = 0.0D;
227 for(int k = 0; k <= j3; k++)
228 {
229 if(flag)
230 matrix.set(k, i, matrix.at(i, k) / d7);
231 double d4 = 0.0D;
232 for(int i2 = 0; i2 <= k; i2++)
233 d4 += matrix.at(k, i2) * matrix.at(i, i2);
234
235 for(int j2 = k + 1; j2 <= j3; j2++)
236 d4 += matrix.at(j2, k) * matrix.at(i, j2);
237
238 matrix2.set(k, 0, d4 / d7);
239 d1 += matrix2.at(k, 0) * matrix.at(i, k);
240 }
241
242 double d8 = d1 / (d7 + d7);
243 for(int l = 0; l <= j3; l++)
244 {
245 double d2 = matrix.at(i, l);
246 double d5 = matrix2.at(l, 0) - d8 * d2;
247 matrix2.set(l, 0, d5);
248 for(int k2 = 0; k2 <= l; k2++)
249 matrix.set(l, k2, matrix.at(l, k2) - (d2 * matrix2.at(k2, 0) + d5 * matrix.at(i, k2)));
250
251 }
252
253 }
254 }
255 else
256 {
257 matrix2.set(i, 0, matrix.at(i, j3));
258 }
259 matrix1.set(i, 0, d7);
260 }
261
262 if(flag)
263 matrix1.set(0, 0, 0.0D);
264 matrix2.set(0, 0, 0.0D);
265 for(int j = 0; j < k3; j++)
266 {
267 if(flag)
268 {
269 j3 = j - 1;
270 if(matrix1.at(j, 0) != 0.0D)
271 {
272 for(int i1 = 0; i1 <= j3; i1++)
273 {
274 double d6 = 0.0D;
275 for(int l2 = 0; l2 <= j3; l2++)
276 d6 += matrix.at(j, l2) * matrix.at(l2, i1);
277
278 for(int i3 = 0; i3 <= j3; i3++)
279 matrix.set(i3, i1, matrix.at(i3, i1) - d6 * matrix.at(i3, j));
280
281 }
282
283 }
284 }
285 matrix1.set(j, 0, matrix.at(j, j));
286 if(flag)
287 {
288 matrix.set(j, j, 1.0D);
289 for(int j1 = 0; j1 <= j3; j1++)
290 {
291 matrix.set(j, j1, 0.0D);
292 matrix.set(j1, j, matrix.at(j, j1));
293 }
294
295 }
296 }
297
298 }
299
300 public Matrix inverse()
301 {
302 if(!isSquare())
303 {
304 System.err.println("Matrix::inverse -> Matrix is not square.");
305 System.exit(0);
306 }
307 int i = _row;
308 Matrix matrix = new Matrix(i, 2 * i);
309 Matrix matrix1 = new Matrix(i, 1);
310 Matrix matrix2 = new Matrix(i, 1);
311 for(int j = 0; j < i; j++)
312 {
313 for(int i2 = 0; i2 < i; i2++)
314 matrix.set(j, i2, _a[j][i2]);
315
316 }
317
318 for(int k = 0; k < i; k++)
319 {
320 for(int j2 = i; j2 < 2 * i; j2++)
321 matrix.set(k, j2, 0.0D);
322
323 matrix.set(k, i + k, 1.0D);
324 }
325
326 for(int i4 = 0; i4 < i; i4++)
327 {
328 for(int l = i4; l < i; l++)
329 {
330 matrix2.set(l, 0, 0.0D);
331 for(int k2 = i4; k2 < i; k2++)
332 matrix2.set(l, 0, matrix2.at(l, 0) + matrix.at(l, k2));
333
334 matrix1.set(l, 0, Math.abs(matrix.at(l, i4)) / matrix2.at(l, 0));
335 }
336
337 int k4 = i4;
338 for(int i1 = i4 + 1; i1 < i; i1++)
339 if(matrix1.at(i1, 0) > matrix1.at(i1 - 1, 0))
340 k4 = i1;
341
342 if(matrix1.at(k4, 0) == 0.0D)
343 {
344 System.err.println("Matrix::inverse -> Matrix is singular.");
345 System.exit(0);
346 }
347 if(k4 != i4)
348 {
349 for(int l2 = 0; l2 < 2 * i; l2++)
350 {
351 double d = matrix.at(i4, l2);
352 matrix.set(i4, l2, matrix.at(k4, l2));
353 matrix.set(k4, l2, d);
354 }
355
356 }
357 for(int i3 = 2 * i - 1; i3 >= i4; i3--)
358 matrix.set(i4, i3, matrix.at(i4, i3) / matrix.at(i4, i4));
359
360 for(int j1 = i4 + 1; j1 < i; j1++)
361 {
362 for(int j3 = 2 * i - 1; j3 >= i4 + 1; j3--)
363 matrix.set(j1, j3, matrix.at(j1, j3) - matrix.at(i4, j3) * matrix.at(j1, i4));
364
365 }
366
367 }
368
369 for(int j4 = i - 1; j4 >= 0; j4--)
370 {
371 for(int k1 = j4 - 1; k1 >= 0; k1--)
372 {
373 for(int k3 = i; k3 < 2 * i; k3++)
374 matrix.set(k1, k3, matrix.at(k1, k3) - matrix.at(j4, k3) * matrix.at(k1, j4));
375
376 }
377
378 }
379
380 Matrix matrix3 = new Matrix(i, i);
381 for(int l1 = 0; l1 < i; l1++)
382 {
383 for(int l3 = 0; l3 < i; l3++)
384 matrix3.set(l1, l3, matrix.at(l1, l3 + i));
385
386 }
387
388 return matrix3;
389 }
390
391 public boolean isLowerDiagonal()
392 {
393 for(int i = 0; i < _row - 1; i++)
394 {
395 for(int j = i + 1; j < _col; j++)
396 if(_a[i][j] != 0.0D)
397 return false;
398
399 }
400
401 return true;
402 }
403
404 public boolean isSquare()
405 {
406 return _row == _col;
407 }
408
409 public boolean isSymmetric()
410 {
411 if(!isSquare())
412 return false;
413 for(int i = 0; i < _row; i++)
414 {
415 for(int j = i + 1; j < _col; j++)
416 if(_a[i][j] != _a[j][i])
417 return false;
418
419 }
420
421 return true;
422 }
423
424 public boolean isUpperDiagonal()
425 {
426 for(int i = 1; i < _row; i++)
427 {
428 for(int j = 0; j < i; j++)
429 if(_a[i][j] != 0.0D)
430 return false;
431
432 }
433
434 return true;
435 }
436
437 public void makeIdentity()
438 {
439 if(_row != _col)
440 {
441 System.err.println("Matrix::makeIdentity -> Matrix is not square.");
442 System.exit(0);
443 }
444 int i = _row;
445 for(int j = 0; j < i; j++)
446 {
447 for(int k = 0; k < i; k++)
448 _a[j][k] = 0.0D;
449
450 _a[j][j] = 1.0D;
451 }
452
453 }
454
455 public Matrix times(double d)
456 {
457 int i = _row;
458 int j = _col;
459 Matrix matrix = new Matrix(i, j);
460 for(int k = 0; k < i; k++)
461 {
462 for(int l = 0; l < j; l++)
463 matrix.set(k, l, d * _a[k][l]);
464
465 }
466
467 return matrix;
468 }
469
470 public Matrix times(Matrix matrix)
471 {
472 int i = _row;
473 int j = _col;
474 int k = matrix.cols();
475 if(_col != matrix.rows())
476 {
477 System.err.println("Matrix::operator* -> Invalid dimensions of matrices.");
478 System.exit(0);
479 }
480 Matrix matrix1 = new Matrix(i, k);
481 for(int l = 0; l < i; l++)
482 {
483 for(int i1 = 0; i1 < k; i1++)
484 {
485 double d = 0.0D;
486 for(int j1 = 0; j1 < j; j1++)
487 d += _a[l][j1] * matrix.at(j1, i1);
488
489 matrix1.set(l, i1, d);
490 }
491
492 }
493
494 return matrix1;
495 }
496
497 protected double pythagoras(double d, double d1)
498 {
499 double d2 = Math.abs(d);
500 double d3 = Math.abs(d1);
501 if(d2 > d3)
502 return d2 * Math.sqrt(1.0D + square(d3 / d2));
503 else
504 return d3 != 0.0D ? d3 * Math.sqrt(1.0D + square(d2 / d3)) : 0.0D;
505 }
506
507
508
509 public int rows()
510 {
511 return _row;
512 }
513
514 public void set(int i, int j, double d)
515 {
516 _a[i][j] = d;
517 }
518
519 public void plusequal(int i, int j, double d)
520 {
521 _a[i][j] += d;
522 }
523
524 public void plusEqual(Matrix b)
525 {
526 for(int i=0; i<rows(); ++i)
527 {
528 for(int j=0; j<cols(); ++j)
529 {
530 _a[i][j] += b._a[i][j];
531 }
532 }
533 }
534
535 protected double square(double d)
536 {
537 if(d == 0.0D)
538 return 0.0D;
539 else
540 return d * d;
541 }
542
543 public Matrix submatrix(int i, int j, int k, int l)
544 {
545 Matrix matrix = new Matrix(i, j);
546 for(int i1 = 0; i1 < i; i1++)
547 {
548 for(int j1 = 0; j1 < j; j1++)
549 matrix._a[i1][j1] = _a[i1 + k][j1 + l];
550
551 }
552
553 return matrix;
554 }
555
556 public Matrix subtract(Matrix matrix)
557 {
558 int i = _row;
559 int j = _col;
560 if(i != matrix.rows() || j != matrix.cols())
561 {
562 System.err.println("Matrix::operator- -> Invalid dimensions of matrices.");
563 System.exit(0);
564 }
565 Matrix matrix1 = new Matrix(i, j);
566 for(int k = 0; k < i; k++)
567 {
568 for(int l = 0; l < j; l++)
569 matrix1.set(k, l, _a[k][l] - matrix.at(k, l));
570
571 }
572
573 return matrix1;
574 }
575
576 public Matrix transposed()
577 {
578 int i = _row;
579 int j = _col;
580 Matrix matrix = new Matrix(j, i);
581 for(int k = 0; k < j; k++)
582 {
583 for(int l = 0; l < i; l++)
584 matrix.set(k, l, _a[l][k]);
585
586 }
587
588 return matrix;
589 }
590
591 public Matrix upperDiagonal()
592 {
593 int i2 = _row;
594 int j2 = _col;
595 if(i2 < j2)
596 {
597 System.err.println("Matrix::upperDiagonal -> More rows than columns required.");
598 System.exit(0);
599 }
600 Matrix matrix = new Matrix(this);
601 int l1 = j2;
602 if(i2 - 1 < j2)
603 l1 = i2 - 1;
604 for(int i1 = 0; i1 <= l1; i1++)
605 {
606 double d1 = 0.0D;
607 for(int i = i1; i < i2; i++)
608 d1 += matrix.at(i, i1) * matrix.at(i, i1);
609
610 double d4 = 0.0D;
611 if(matrix.at(i1, i1) < 0.0D)
612 d4 = -1D;
613 if(matrix.at(i1, i1) > 0.0D)
614 d4 = 1.0D;
615 double d2 = -d4 * Math.sqrt(d1);
616 double d = d1 - matrix.at(i1, i1) * d2;
617 matrix.set(i1, i1, matrix.at(i1, i1) - d2);
618 for(int k1 = i1 + 1; k1 < j2; k1++)
619 {
620 double d3 = 0.0D;
621 for(int j = i1; j < i2; j++)
622 d3 += matrix.at(j, i1) * matrix.at(j, k1);
623
624 double d5 = d3 / d;
625 for(int k = i1; k < i2; k++)
626 matrix.set(k, k1, matrix.at(k, k1) - matrix.at(k, i1) * d5);
627
628 }
629
630 matrix.set(i1, i1, d2);
631 }
632
633 for(int l = 1; l < i2; l++)
634 {
635 for(int j1 = 0; j1 < l; j1++)
636 matrix.set(l, j1, 0.0D);
637
638 }
639
640 if(matrix.at(j2 - 1, j2 - 1) == 0.0D)
641 {
642 System.err.println("Matrix::householder -> Rank of matrix < n");
643 System.exit(0);
644 }
645 int k2 = _row;
646 if(_col < _row)
647 k2 = _col;
648 return matrix.submatrix(k2, k2, 0, 0);
649 }
650
651 public void write(String s)
652 {
653 write(s, "");
654 }
655
656 public void write(String s, String s1)
657 {
658 System.out.println("Writing matrix to '" + s + "'.");
659 try
660 {
661 PrintWriter printwriter = new PrintWriter(new BufferedWriter(new FileWriter(s)));
662 String s2 = "# " + s1;
663 printwriter.println(s2);
664 String s3 = _row + " " + _col;
665 printwriter.println(s3);
666 String s4 = "";
667 for(int i = 0; i < _row; i++)
668 {
669 String s5 = "";
670 for(int j = 0; j < _col; j++)
671 s5 = s5 + _a[i][j] + " ";
672
673 printwriter.println(s5.substring(0, s5.length() - 1));
674 }
675
676 printwriter.close();
677 }
678 catch(IOException _ex)
679 {
680 System.err.println("Matrix::write -> Error writing to '" + s + "'.");
681 System.exit(0);
682 }
683 System.out.println("Matrix written.");
684 }
685
686 public String toString()
687 {
688 return "Matrix: ";
689 }
690
691 public Matrix column(int col)
692 {
693 int rows = _row;
694 int cols = 1;
695 Matrix matrix = new Matrix(rows, cols);
696 for(int i = 0; i < rows; ++i)
697 {
698 matrix.set(i, 0, _a[i][col]);
699 }
700
701 return matrix;
702 }
703
704 public Matrix row(int row)
705 {
706 int rows = 1;
707 int cols = _col;
708 Matrix matrix = new Matrix(rows, cols);
709 for(int i = 0; i < cols; ++i)
710 {
711 matrix.set(0, i, _a[row][i]);
712 }
713
714 return matrix;
715 }
716 }