Rename for C++
[openmx:openmx.git] / src / matrix.cpp
1 #include <math.h>
2 #include <float.h>               
3 #include <stdio.h>
4 #include <stdlib.h>
5 #include <stdbool.h>
6 #include "matrix.h"
7
8 double rnd_double() { return (double)1.0; }
9
10 Matrix new_matrix(int cols,int rows)
11 {
12         Matrix t;
13         t.rows=rows;
14         t.cols=cols;
15         t.isColMajor = 1;
16         t.t=(double *)malloc(sizeof(double)*cols*rows);
17         
18         int i,j;
19         for(i=0;i<rows;i++){
20                 for(j=0;j<cols;j++) {
21                         M(t,j,i)=rnd_double();
22                 }
23         }
24         return t;
25 }
26
27 Matrix fill(int cols, int rows, double value){
28         Matrix t = new_matrix(cols, rows);
29         int i,j;
30         for(i=0;i<rows;i++){
31                 for(j=0;j<cols;j++) {
32                         M(t,j,i)=value;
33                 }
34         }
35         return t;
36 }
37
38 Matrix getRow( Matrix t, int row){
39         printf("t.cols is: \n");
40     printf("%d", t.cols); putchar('\n');
41     printf("t,0,0 is: \n");
42     printf("%2f", M(t,0,0)); putchar('\n');
43     
44
45         Matrix toReturn = fill(t.cols, 1, (double)0.0);
46     printf("toReturn is: \n");
47     printf("%2f", M(toReturn, 0, 0)); putchar('\n');
48
49         int i;
50         for (i=0;i < t.cols; i++){
51                 M(toReturn,i,0) = M(t,i,row);
52        // printf("toReturn is: \n");
53        // print(toReturn); putchar('\n');
54         }
55         
56         return toReturn;
57 }
58
59 Matrix setRow( Matrix x, int row,  Matrix y){
60         
61         Matrix toReturn = duplicateIt(x);
62         
63         int i,j;
64         
65         for (i=0;i < x.cols; i++){
66                 M(toReturn,i,row) = M(y,i,0);
67         }
68         
69         return toReturn;
70 }
71
72 Matrix getColumn( Matrix t, int colNum)
73 {       
74    int r, c;
75    int index = 0;
76     Matrix result = fill(t.rows, 1, (double)0.0);
77    for ( r = 0; r < t.rows; ++r )
78    {
79        for ( c = 0; c < t.cols; ++c )
80        {
81                 if (c==colNum){
82                                 M(result, index, 0) = M(t, c, r);
83                                 index++; 
84                         }
85        }
86    }
87    return result;
88 }
89
90 Matrix setColumn( Matrix x,  Matrix y, int colNum)
91 {
92    int r, c;
93    int index = 0;
94     Matrix result = fill(x.cols, x.rows, (double)0.0);
95    for ( r = 0; r < x.rows; ++r )
96    {
97       for ( c = 0; c < x.cols; ++c )
98       {
99                 if (c==colNum){
100                         M(result, c, r) = M(y, index, 0);
101                         index++; 
102                 }
103                 else{
104                         M(result, c, r) = M(x, c, r);
105                 }
106       }
107    }
108    return result;
109 }
110
111 void print(Matrix t) {
112         int i,j;
113         for(i=0;i<t.rows;i++) {
114                 printf("| ");
115                 for(j=0;j<t.cols;j++) 
116                         printf("%.16f ",M(t,j,i));
117                 printf("|\n");
118         }
119         printf("\n");
120 }
121
122 Matrix matrix_mult(Matrix a, Matrix b) {
123         int x,y,z;
124          Matrix r;
125         r=new_matrix(a.rows,a.rows);
126
127         for(x=0;x<a.rows;x++)
128                 for(y=0;y<b.cols;y++) {
129                         M(r,y,x)=0.0;
130                         for(z=0;z<a.cols;z++) {
131                                 M(r,y,x)+=M(a,z,x)*M(b,y,z);
132                         }
133                 }
134         return r;
135 }
136
137 Matrix solve(Matrix X,  Matrix y){
138         Matrix A = duplicateIt(X);
139         Matrix b = duplicateIt(y);
140         int N = b.cols;
141         /** b is a 1 row matrix **/
142         int p, i, j;
143         for (p=0; p<N; p++){
144                 // find pivot row and swap
145         int max = p;
146                 for (i = p + 1; i<N; i++) {
147             if ( abs(M(A,p,i)) > abs(M(A,p,max)) ) {
148                 max = i;
149             }
150         }
151
152                 Matrix temp = fill(A.cols, 1, (double)0.0);
153                 
154                 temp = getRow(A,p); A = setRow(A,p, getRow(A,max)); 
155                 A = setRow(A,max, temp);
156                 double t = M(b,p,0); M(b,p,0) = M(b,max,0); M(b,max,0)= t;
157                 
158
159                 // pivot within A and b
160         for (i = p + 1; i < N; i++) {   
161                         double alpha = (M(A,p,i)/M(A,p,p));
162                         M(b,i,0) -= alpha * M(b,p,0);   
163             for (j = p; j < N; j++) {
164                                 M(A,j,i) -= alpha * M(A,j,p);
165             }
166         }
167         }
168
169         // back substitution
170         Matrix x = fill(N, 1, (double)0.0);
171         for (i = (N - 1); i >= 0; i--) {
172             double sum = 0.0;
173             for (j = i + 1; j < N; j++) {
174                         sum += M(A,j,i) * M(x,j,0);
175              }
176                  M(x, i, 0) = (M(b, i, 0) - sum) / M(A, i, i);
177          }
178          return duplicateIt(x); 
179
180
181 // return Cholesky factor L of psd matrix A = L L^T
182 Matrix cholesky(Matrix A){
183          Matrix L = fill(A.rows, A.cols, (double)0.0);
184         int i,j, k;
185         for (i=0; i<A.cols; i++){
186                 for (j=0; j<= i; j++){
187                         double sum = 0.0;
188                         for (k=0; k<j; k++){
189                                 sum += (M(L,k,i) * M(L,k,j));
190             }
191             if (i == j) {
192                                 M(L,i,i) = sqrt(M(A,i,i) - sum);
193                         }       
194             else{
195                                 M(L,j,i) = 1.0 / M(L,j,j) * (M(A,j,i) - sum);
196                         }
197                 }
198         }
199         return L;
200 }
201
202 Matrix diag(Matrix A){
203            int i,j;
204             Matrix result = fill(A.cols, A.cols, (double)0.0);
205            for (i=0; i<A.cols; i++){
206                         for (j=0; j<=A.cols; j++){
207                                  if (i==j)
208                                         M(result, j, i) = M(A, j, 0);
209                         }
210            }            
211            return result;
212 }
213
214 //Mahsa: this was wrong. it was "value <= M(t,j,i)", and "j<=t.cols". both are wrong.
215 bool allGreaterThan(Matrix t, double value){
216         int i,j;
217         for (i=0; i<t.rows; i++){
218                 for (j=0; j<t.cols; j++){
219                 if (value >= M(t,j,i)){
220                                 return false;
221                         }
222         }
223         }
224         return true;
225 }
226
227 double vnorm(Matrix t){
228    double result = 0;
229    int i,j;
230
231    for ( i = 0; i < t.rows; ++i )
232    {
233       for ( j = 0; j < t.cols; ++j )
234       {
235                 result += (M(t,j,i) * M(t,j,i));
236       }
237         }
238         
239         return sqrt(result);
240 }
241
242 double findMin(Matrix t){
243    int i,j;
244    double min = DBL_MAX;
245    for ( i = 0; i < t.rows; ++i )
246    {
247       for ( j = 0; j < t.cols; ++j )
248       {
249          if ( min > M(t, j, i) ){
250                         min = M(t, j, i);
251                  }
252       }
253         }
254         return min;
255 }
256
257 double findMax(Matrix t){
258    int i,j;
259    double max = -999999.0;
260
261    for ( i = 0; i < t.rows; ++i )
262    {
263       for ( j = 0; j < t.cols; ++j )
264       {
265          if (max < M(t,j,i) ){
266                         max = M(t,j,i);
267              //printf("max is:"); putchar('\n');
268              //printf("%2f", max); putchar('\n');
269                  }
270       }
271         }
272         return max;
273 }
274
275 double min(double a, double b){
276         if (a < b){
277                 return a;
278         }
279         else {
280                 return b;
281         }
282 }
283
284 double ourAbs(double a){
285         if (a < 0.0){
286                 return (a * -1.0);
287         }
288         else {
289                 return a;
290         }
291 }
292
293 double max(double a, double b){
294         if (a > b){
295                 return a;
296         }
297         else {
298                 return b;
299         }
300 }
301
302
303 double dotProduct(Matrix a, Matrix b) { //Mahsa: a.cols element of matrix a is multiplied by a.cols first element of matrix b. e.g. if matrix a is 2 by 4, and matrix b is the transpose of matrix a (hence 4 by 2), then dotProduct multiplied first row of matrix a by first row and second row of matrix b (2+2 = 4 first elements of matrix b), and adds them all together.
304         double runningSum = 0;
305         int index;
306         for (index = 0; index < a.cols; index++)
307     {   
308                 runningSum += M(a, index, 0) * M(b, index, 0);
309     }
310         return runningSum;
311 }
312
313 Matrix matrixDotProduct(Matrix a,  Matrix b){
314    int r;
315     Matrix result = fill(a.cols, 1, (double)0.0);
316    for ( r = 0; r < a.rows; ++r )
317    {
318        
319             M(result, r, 0) = dotProduct(getRow(a, r), getRow(b, 0));
320    }
321    return result;       
322 }
323
324 Matrix minMaxAbs(Matrix t, double tol)
325 {
326    // for each x[i]: min( max( abs(x[i]), tol ), 1/tol )        
327    int r,c;
328     Matrix result = fill(t.cols, t.rows, (double)0.0);
329    for ( r = 0; r < t.rows; ++r )
330    {
331       for ( c = 0; c < t.cols; ++c )
332       {
333                 double buf = ourAbs(M(t,c,r));
334                 buf = max(buf, tol);
335                 buf = min(buf, 1.0/(tol));
336                 M(result,c,r) = buf;
337       }
338    }
339    return result;
340 }
341
342 Matrix add(Matrix x,  Matrix y)
343 {
344     Matrix result = fill(x.cols, x.rows, (double)0.0);
345    int r,c;
346    for ( r = 0; r < x.rows; ++r )
347    {
348       for ( c = 0; c < x.cols; ++c )
349       {
350                   M(result, c, r) = M(x, c, r) + M(y, c, r);
351       }
352    }
353    return result;
354 }
355
356 Matrix subtract(Matrix x,  Matrix y)
357 {
358     Matrix result = fill(x.cols, x.rows, (double)0.0);
359    int r,c;
360    for ( r = 0; r < x.rows; ++r )
361    {
362       for ( c = 0; c < x.cols; ++c )
363       {
364                   M(result, c, r) = M(x, c, r) - M(y, c, r);
365       }
366    }
367    return result;
368 }
369
370 Matrix multiply(Matrix x,  Matrix y)
371 {
372     Matrix result = fill(x.cols, x.rows, (double)0.0);
373    int r,c;
374    for ( r = 0; r < x.rows; ++r )
375    {
376       for ( c = 0; c < x.cols; ++c )
377       {
378                   M(result, c, r) = M(x, c, r) * M(y, c, r);
379       }
380    }
381    return result;
382 }
383
384 Matrix divide(Matrix x,  Matrix y)
385 {
386     Matrix result = fill(x.cols, x.rows, (double)0.0);
387    int r,c;
388    for ( r = 0; r < x.rows; ++r )
389    {
390       for ( c = 0; c < x.cols; ++c )
391       {
392                   M(result, c, r) = M(x, c, r) / M(y, c, r);
393       }
394    }
395    return result;
396 }
397
398 Matrix copyInto(Matrix x,  Matrix y, int rowNum, int colStart, int colStop){
399         
400            int r, c, index;
401            index = 0;
402             Matrix result = fill(x.cols, x.rows, (double)0.0);
403            for ( r = 0; r < x.rows; ++r )
404            {
405               for ( c = 0; c < x.cols; ++c )
406               {
407                         if (r == rowNum){
408                                 if (c >= colStart && c <= colStop){
409                                         M(result, c, r) = M(y, index, 0);
410                                         index++;
411                                 }
412                                 else{
413                                         M(result, c, r) = M(x, c, r);
414                                 }
415                         }
416                         else{
417                                         M(result, c, r) = M(x, c, r);
418                         }  
419               }
420            }
421            return result;
422 }
423
424 Matrix rowWiseMin(Matrix t)
425 {
426     Matrix mins = fill(t.rows, 1, (double)0.0);
427    int r,c;
428    for ( r = 0; r < t.rows; ++r )
429    {
430       double min = DBL_MAX;
431       for ( c = 0; c < t.cols; ++c )
432       {
433                    if ( M(t,c,r) < min){
434                           min = M(t,c,r);
435                    }
436           }
437           M(mins, r, 0) = min;
438    }
439    return mins;
440 }
441
442 Matrix transpose2D(Matrix t){ //Mahsa: 1) first row is copied into a matrix with the size equal to the argument matrix. then each element of this row is multiplied by the corresponding row in the copied matrix. i.e first element is multiplied by the first row. second element is multiplied by the second row, and so on. 
443    int r, c;
444    Matrix result = fill(t.cols,t.cols,(double)0.0);
445    for ( r = 0; r < t.cols; ++r )
446    {
447       for ( c = 0; c < t.cols; ++c )
448       {
449                         M(result,c,r) = M(t,c,0);
450       }
451    }
452     //printf("result: transpose2D: \n");
453     //print(result); putchar('\n');
454    for ( r = 0; r < t.cols; ++r )
455    {
456       for ( c = 0; c < t.cols; ++c )
457       {
458                         M(result,c,r) = M(result,c,r) * M(t,r,0);
459       }
460    }
461
462    return result;
463
464 }
465
466 Matrix transposeDotProduct(Matrix t){ //Mahsa: 1) takes the minimum dimension. 2) multiplies the first row by itself and the other rows(as far as the minimum dimension allows). multiplies the second row by itself and the rest, and so on. This gives the same answer as "a%*%t(a)" in R. 
467         int r,c;
468         double minDim = min(t.cols, t.rows);
469         Matrix result = fill(minDim,minDim,(double)0.0);
470         
471     for ( r = 0; r < minDim; ++r )
472     {
473         for ( c = 0; c < minDim; ++c )
474         {
475                         M(result,c,r) = dotProduct(getRow(t,r), getRow(t,c));
476                 }
477         }
478         return result;
479 }
480
481 Matrix transposeDP(Matrix t){ //Mahsa: same as transpose2D function
482     Matrix toReturn = fill(t.cols, t.cols, (double)0.0);
483    int r, c;
484    for ( r = 0; r < t.cols; ++r )
485    {
486       for ( c = 0; c < t.cols; ++c )
487           {
488                   M(toReturn, c, r) = M(t, r, 0) * M(t, c, 0);
489           }
490    }
491    return toReturn;
492 }
493
494 Matrix transpose(Matrix t){ // Mahsa: simply transposes the matrix
495          Matrix result = fill(t.rows, t.cols, (double)0.0);
496         int r,c;
497         for ( r = 0; r < t.cols; ++r )
498     {
499                 for ( c = 0; c < t.rows; ++c )
500          {
501                         M(result, c, r) = M(t, r, c);
502          }
503     }
504         return result;
505 }
506
507 Matrix negate(Matrix t)
508 {
509    int r, c;
510     Matrix result = fill(t.cols, t.rows, (double)0.0);
511    for ( r = 0; r < t.rows; ++r )
512    {
513         for ( c = 0; c < t.cols; ++c )
514         {
515                         M(result, c, r) = (M(t, c, r) * -1.0);
516         }
517    }
518    return result;
519 }
520
521 Matrix duplicateIt(Matrix t)
522 {
523    int r, c;
524     Matrix result = fill(t.cols, t.rows, (double)0.0);
525    for ( r = 0; r < t.rows; ++r )
526    {
527         for ( c = 0; c < t.cols; ++c )
528         {
529                         M(result, c, r) = M(t, c, r);
530         }
531    }
532    return result;
533 }
534
535 Matrix matrixAbs(Matrix t)
536 {
537    int r, c;
538     Matrix result = fill(t.cols, t.rows, (double)0.0);
539    for ( r = 0; r < t.rows; ++r )
540    {
541         for ( c = 0; c < t.cols; ++c )
542         {
543                         M(result, c, r) = ourAbs(M(t, c, r));
544         }
545    }
546    return result;
547 }
548
549 Matrix multiplyByScalar2D(Matrix t, double multiplier)
550 {
551    int r, c;
552     Matrix result = fill(t.cols, t.rows, (double)0.0);
553    for ( r = 0; r < t.rows; ++r )
554    {
555         for ( c = 0; c < t.cols; ++c )
556         {
557                         M(result, c, r) = M(t, c, r)*multiplier;
558         }
559    }
560    return result;
561 }
562
563 Matrix divideByScalar2D(Matrix t, double divisor)
564 {
565    int r, c;
566     Matrix result = fill(t.cols, t.rows, (double)0.0);
567    for ( r = 0; r < t.rows; ++r )
568    {
569         for ( c = 0; c < t.cols; ++c )
570         {
571                         M(result, c, r) = M(t, c, r)/divisor;
572         }
573    }
574    return result;
575 }
576
577 Matrix checkControlList(Matrix t){
578     Matrix result = fill(t.cols, t.rows, (double)0.0);
579    int length = t.cols;
580    int c;   
581
582    M(result, 0, 0)=1;
583    M(result, 1, 0)=400;
584    M(result, 2, 0)=800;
585    M(result, 3, 0)=1.0e-7;
586    M(result, 4, 0)=1.0e-8;
587    M(result, 5, 0)=1;
588    
589         for ( c = 0; c < length; ++c )
590     {
591                 M(result, c, 0) = M(t, c, 0);
592     }
593         return result;
594 }
595
596 Matrix subset(Matrix t, int row, int colStart, int colStop)
597 {
598
599    int r, c;
600    int count = (colStop - colStart)+1;
601    int index = 0;
602    
603     Matrix result = fill(count, 1, (double)0.0);        
604    for ( r = 0; r < t.rows; ++r )
605    {
606       for ( c = 0; c < t.cols; ++c )
607       {
608               if (r == row){
609                           if ( (c >= colStart) && (c <= colStop) ){
610                                 M(result, index, 0) = M(t, c, r);
611                             index++;
612                           }
613                   }     
614       }
615    }
616    return result;               
617 }
618
619
620 Matrix copy(Matrix x,  Matrix y){
621         
622         int totalRows = x.rows;
623     int totalCols = x.cols + y.cols;
624     Matrix result = fill(totalCols, totalRows, (double)0.0);
625         
626         int r, c;
627         for ( r = 0; r < totalRows; ++r )
628      {
629         for ( c = 0; c < totalCols; ++c )
630         {
631                         if (c < x.cols){
632                                 M(result, c, r) = M(x, c, r);
633                         }
634                         else {
635                                 M(result, c, r) = M(y, (c-x.cols), r);
636                         }
637                 }
638          }      
639         return result;
640 }
641
642 Matrix rbind(Matrix x,  Matrix y){
643          Matrix result = copy(x, y);
644         return result;
645 }
646
647 Matrix copyThree(Matrix a,  Matrix b,  Matrix c){
648     Matrix result = copy(a, b);
649     Matrix result_three = copy(result, c);
650 }
651
652 Matrix copyFive(Matrix a,  Matrix b,  Matrix c,  Matrix d,  Matrix e){
653         copy(copy(copy(a,b), copy(c,d)), e);
654 }
655
656 Matrix timess(Matrix a,  Matrix b){
657         printf("times is: \n");
658         int i, j, k;
659          Matrix result = fill(b.cols, a.rows, (double)0.0);
660          Matrix Bcolj = fill(a.cols, 1, (double)0.0);
661         for (j=0; j<b.cols; j++){
662                 for (k=0; k<a.cols; k++){
663                         M(Bcolj, k, 0) = M(b, j, k);
664                 }
665                 for (i=0; i<a.rows; i++){
666                         double s = 0;
667                         for (k=0; k<a.cols; k++){
668                                 s+= M(a, k, i) * M(Bcolj, k, 0);
669                         }
670                         M(result, j, i) = s;
671                 }
672         }
673         printf("result is: \n");
674         print(result);
675         return result;
676 }
677
678 Matrix luSolve(Matrix a,  Matrix b){
679         int m, n, pivsign;
680         double EMPTY = -999999.0;
681         m = a.rows;
682         n = a.cols;
683         
684         Matrix LU = duplicateIt(a);
685         Matrix piv = fill(m, 1, EMPTY);
686         
687         int i, j, k;
688         for (i = 0; i < m; i++) {
689          M(piv, i, 0) = i;
690     }
691
692     pivsign = 1;
693         
694         // Outer loop
695         
696     for (j = 0; j < n; j++) {
697
698          // Apply previous transformations.
699          for (i = 0; i < m; i++) {
700
701             // Most of the time is spent in the following dot product.
702             int kmax = min(i,j);
703             double s = 0.0;
704             for (k = 0; k < kmax; k++) {
705                                 s += M(LU, k, i) * M(LU,j, k);
706             }
707                         M(LU,j,i) = M(LU,j,i) -= s;
708          }
709    
710          // Find pivot and exchange if necessary.
711          int p = j;
712          for (i = j+1; i < m; i++) {
713             if ( abs(M(LU,j, i)) > abs(M(LU,j, p)) ){
714                                 p = i;
715             }
716          }
717
718          if (p != j) {
719             for (k = 0; k < n; k++) {
720                                 double t = M(LU, k, p); M(LU, k, p) = M(LU, k, j); M(LU, k, j) = t;
721             }
722                         int k = M(piv, p, 0); M(piv, p, 0) = M(piv, j, 0); M(piv, j, 0) = k;
723             pivsign = -1 * pivsign;
724          }
725
726
727          // Compute multipliers.
728          
729          if (j < m & M(LU, j, j) != 0.0) {
730             for (i = j+1; i < m; i++) {
731                                 M(LU, j, i) /= M(LU, j, j);
732             }
733          }
734       }
735
736
737         int nx = b.rows;
738         // array of row indices
739         int pivLength = m;
740         
741         for (i=0; i<m; i++){
742                 if (M(piv, i, 0) == EMPTY){
743                         pivLength = i;
744                 }
745         }
746         
747         // Copy right hand side with pivoting
748          Matrix X = fill(nx, pivLength, (double)0.0);
749         
750         
751         for (i=0; i<pivLength; i++){
752                 for (j=0; j<nx; j++){
753                         int index = M(piv, i, 0);
754                         M(X, j, i) = M(b, j, index);
755                 }
756         }
757         
758
759     // Solve L*Y = B(piv,:)
760     for (k = 0; k < n; k++) {
761        for (i = k+1; i < n; i++) {
762           for (j = 0; j < nx; j++) {
763                         M(X, j, i) -= M(X, j, k)*M(LU, k, i);
764            }
765         }
766     }
767
768     // Solve U*X = Y;
769     for (k = n-1; k >= 0; k--) {
770        for (j = 0; j < nx; j++) {
771                         M(X, j, k) /= M(LU, k, k);
772        }
773        for (i = 0; i < k; i++) {
774           for (j = 0; j < nx; j++) {
775                         M(X, j, i) -= M(X, j, k)*M(LU, k, i);
776           }
777        }
778     }
779         return duplicateIt(X);
780 }
781
782 Matrix qrSolve(Matrix a,  Matrix b){
783           // Initialize.
784         int m = a.rows;
785         int n = a.cols;
786         
787          Matrix QR = duplicateIt(a);
788          Matrix Rdiag = fill(n, 1, (double)0.0);
789
790     // Main loop.
791         int k, i, j;
792     for (k = 0; k < n; k++) {
793         // Compute 2-norm of k-th column without under/overflow.
794                 double nrm = 0;
795         for (i = k; i < m; i++) {
796                         nrm = hypot(nrm, M(QR, k, i));
797         }
798         if (nrm != 0.0) {
799             // Form k-th Householder vector.
800             if (M(QR, k, k) < 0) {
801                nrm = -1*nrm;
802             }
803             for (i = k; i < m; i++) {
804                M(QR, k, i) /= nrm;
805             }
806             M(QR,k,k) += 1.0;
807
808             // Apply transformation to remaining columns.
809             for (j = k+1; j < n; j++) {
810                double s = 0.0; 
811                for (i = k; i < m; i++) {
812                                   s += M(QR, k, i) * M(QR, j, i);
813                }
814                            s = -s/M(QR, k, k);
815                for (i = k; i < m; i++) {
816                                   M(QR, j, i) += s * M(QR, k, i);
817                }
818             }
819          }
820                  M(Rdiag, k, 0) = -1 * nrm;
821     }
822         int nx = b.cols;
823          Matrix X = duplicateIt(b);
824           // Compute Y = transpose(Q)*B
825       for (k = 0; k < n; k++) {
826          for (j = 0; j < nx; j++) {
827             double s = 0.0; 
828             for (i = k; i < m; i++) {
829                                 s += M(QR, k, i) * M(X, j, i);
830             }
831                         s = (-1*s)/M(QR, k, k);
832             for (i = k; i < m; i++) {
833                                 M(X, j, i) += s * M(QR, k, i);
834             }
835          }
836       }
837       // Solve R*X = Y;
838       for (k = n-1; k >= 0; k--) {
839          for (j = 0; j < nx; j++) {
840                         M(X, j, k) /= M(Rdiag, k, 0);
841          }
842          for (i = 0; i < k; i++) {
843             for (j = 0; j < nx; j++) {
844                                 M(X, j, i) -= M(X, j, k) * M(QR, k, i);
845             }
846          }
847       }
848         return duplicateIt(X);
849 }
850
851 //
852 // Creates an orthogonal matrix Q such that QA = R, i.e. A = Q^T R. A may be rectangular.
853 // @param matrix    Matrix of size n x m
854 // @param q         Result Matrix of size n x n (orthognal)
855 // @param r         Result Matrix of size n x m (upper right)
856 // @param work      Work Vector of size n
857
858 Matrix qrDecomposition(Matrix t, bool rDecomp){
859         
860         int m = t.rows;
861         int n = t.cols;
862         
863          Matrix r = fill(m, n, (double)0.0);
864          Matrix q = fill(m, n, (double)0.0);
865         
866          Matrix work = fill(n, 1, (double)0.0);
867         
868         int i, j, k;
869
870                 for (i=0; i<n; i++){
871                         for (j=0; j<m; j++){
872                                 M(r, j, i) = M(t, j, i);
873                         }
874                 }
875                 
876                 for (i=0; i<n; i++){
877                         for (j=0; j<m; j++){
878                                 if (i==j){
879                                         M(q, j, i) = 1;
880                                 }
881                         }
882                 }
883
884     // Looping through columns for Householder Transformations
885     for (i=0; i<min(m,n); i++) {
886         // Preparing lambda fro Householder
887         double lambda = 0; for (j=i; j<n; j++) lambda += M(r, i, j) * M(r, i, j);
888         lambda = sqrt(lambda);
889         if (M(r, i, i) < 0) lambda = -lambda;
890
891         // compute Householder
892                 M(work, i, 0) = M(r, i, i) + lambda; for (j=i+1; j<n; j++) M(work, j, 0) = M(r, i, j);
893                 double denom = 0; for (j=i; j<n; j++) denom += M(work, j, 0) * M(work, j, 0);
894
895         // Apply Householder to result triangular matrix
896         for (j=i; j<m; j++) {
897                         double p = 0; for (k=i; k<n; k++) p+= M(work, k, 0) * M(r, j, k);
898                         for (k=i; k<n; k++) M(r, j, k) -= 2*M(work, k, 0) * p/denom;
899         }
900
901         // Apply Householder to result orthogoal matrix
902         for (j=0; j<n; j++) {
903                         double p = 0; for (k=i; k<n; k++) p += M(work, k, 0) * M(q, j, k);
904             for (k=i; k<n; k++) M(q, j, k) -= 2*M(work, k, 0) * p/denom;
905         }
906     }
907
908         if (rDecomp){
909                         return r;
910         }
911         else{
912                         return q;
913         }
914 }
915
916 Matrix rowSort(Matrix t)
917 {
918    int r, c, i, j;
919     Matrix result = fill(t.cols, t.rows, (double)0.0);
920    for ( r = 0; r < t.rows; ++r )
921    {
922       for ( c = 0; c < t.cols; ++c )
923       {
924                 M(result, c, r) = M(t, c, r);
925       }
926         //for ( r = 0; r < t.rows; ++r )
927         //{
928                 for ( i = 0; i < t.cols; ++i )
929                 {
930                                 for ( j = 0; j < t.cols; ++j )
931                         {
932                                         if (M(result, i, r) < M(result, j, r)){
933                                                 double a = M(result, i, r);
934                                                 M(result, i, r) = M(result, j, r);
935                                                 M(result, j, r) = a;
936                                         }
937                         }
938                 //}
939         }
940    }
941    return result;
942 }
943
944 Matrix QRd(Matrix mainMat, Matrix RHSMat)
945 {
946     int lwork = 4 * mainMat.rows * mainMat.cols;
947         int l = 0;
948     char TRANS = 'N';
949         Matrix result;
950     result = duplicateIt(RHSMat);
951         double* work = (double*) malloc(lwork * sizeof(double));
952     
953     F77_CALL(dgels)(&TRANS, &(mainMat.rows), &(mainMat.cols), &(result.cols), mainMat.t, &(mainMat.rows), result.t, &(result.rows), work, &lwork, &l);
954     //result = subset(result, 0, 0, 1);
955     return result;
956 }
957
958
959 Matrix MatrixInvert(Matrix inMat)
960 {
961         Matrix result;
962         
963         int lwork = 4 * inMat.rows * inMat.cols;
964         int l = 0;
965
966         int*    ipiv = (int*) malloc(inMat.rows * sizeof(int));
967         double* work = (double*) malloc(lwork * sizeof(double));
968
969         result = duplicateIt(inMat);
970         F77_CALL(dgetrf)(&(result.cols), &(result.rows), result.t, &(result.rows), ipiv, &l);
971         if(l != 0) {
972                 char *errstr = calloc(250, sizeof(char));
973                 sprintf(errstr, "Attempted to invert non-invertable matrix.");
974                 //omxRaiseError(result->currentState, -1, errstr);
975                 free(errstr);
976         } else {
977                 F77_CALL(dgetri)(&(result.cols), result.t, &(result.rows), ipiv, work, &lwork, &l);
978         }
979
980         free(ipiv);
981         free(work);
982         return result;
983 }
984
985 int this_main(int argc,char *argv[]) {
986         int i,j;
987         int size=atoi(argv[1]);
988          Matrix a,b,c;
989
990         a=new_matrix(size,size);
991         b=new_matrix(size,size);
992         for(i=0;i<size;i++){
993                 for(j=0;j<size;j++) {
994                         M(a,i,j)=rnd_double();
995                         M(b,i,j)=rnd_double();
996                 }
997         }
998         c=matrix_mult(a,b);
999         print(c);
1000 }
1001
1002