Remove unused argument from omxResizeMatrix
[openmx:openmx.git] / src / omxMatrix.cpp
1 /*
2  *  Copyright 2007-2014 The OpenMx Project
3  *
4  *  Licensed under the Apache License, Version 2.0 (the "License");
5  *  you may not use this file except in compliance with the License.
6  *  You may obtain a copy of the License at
7  *
8  *       http://www.apache.org/licenses/LICENSE-2.0
9  *
10  *  Unless required by applicable law or agreed to in writing, software
11  *  distributed under the License is distributed on an "AS IS" BASIS,
12  *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  *  See the License for the specific language governing permissions and
14  *  limitations under the License.
15  */
16
17 /***********************************************************
18 *
19 *  omxMatrix.cc
20 *
21 *  Created: Timothy R. Brick    Date: 2008-11-13 12:33:06
22 *
23 *       Contains code for the omxMatrix class
24 *   omxDataMatrices hold necessary information to simplify
25 *       dealings between the OpenMX back end and BLAS.
26 *
27 **********************************************************/
28 #include "omxMatrix.h"
29 #include "omxOpenmpWrap.h"
30 #include "matrix.h"
31
32 // forward declarations
33 static omxMatrix* fillMatrixHelperFunction(omxMatrix* om, SEXP matrix, omxState* state,
34         unsigned short hasMatrixNumber, int matrixNumber);
35
36 const char omxMatrixMajorityList[3] = "Tn";             // BLAS Column Majority.
37
38 void omxPrintMatrix(omxMatrix *source, const char* header)
39 {
40         std::string buf;
41         buf += string_snprintf("[%d] %s %s = matrix(c(    # %dx%d",
42                                omx_absolute_thread_num(),
43                                header, source->name? source->name : "?", source->rows, source->cols);
44
45         int first=TRUE;
46         if(source->colMajor) {
47                 for(int j = 0; j < source->rows; j++) {
48                         buf += "\n";
49                         for(int k = 0; k < source->cols; k++) {
50                                 if (first) first=FALSE;
51                                 else buf += ",";
52                                 buf += string_snprintf(" %3.6f", source->data[k*source->rows+j]);
53                         }
54                 }
55         } else {
56                 for(int j = 0; j < source->cols; j++) {
57                         buf += "\n";
58                         for(int k = 0; k < source->rows; k++) {
59                                 if (first) first=FALSE;
60                                 else buf += ",";
61                                 buf += string_snprintf(" %3.6f", source->data[k*source->cols+j]);
62                         }
63                 }
64         }
65
66         buf += string_snprintf("), byrow=TRUE, nrow=%d, ncol=%d)\n", source->rows, source->cols);
67         mxLogBig(buf);
68 }
69
70 omxMatrix* omxInitMatrix(int nrows, int ncols, unsigned short isColMajor, omxState* os) {
71
72         if (!isColMajor) Rf_error("All matrices are created column major");
73
74         omxMatrix* om = new omxMatrix;
75
76         om->hasMatrixNumber = 0;
77         om->rows = nrows;
78         om->cols = ncols;
79         om->colMajor = (isColMajor ? 1 : 0);
80
81         om->originalRows = om->rows;
82         om->originalCols = om->cols;
83
84         om->owner = NULL;
85         if(om->rows == 0 || om->cols == 0) {
86                 om->data = NULL;
87         } else {
88                 om->data = (double*) Calloc(nrows * ncols, double);
89         }
90
91         om->algebra = NULL;
92         om->fitFunction = NULL;
93
94         om->currentState = os;
95         om->name = NULL;
96         om->version = 1;
97         omxMarkClean(om);
98
99         omxMatrixLeadingLagging(om);
100
101         return om;
102
103 }
104
105 static void omxFreeInternalMatrixData(omxMatrix * om)
106 {
107         if(!om->owner && om->data != NULL) {
108                 Free(om->data);
109         }
110         om->owner = NULL;
111         om->data = NULL;
112 }
113
114 void omxCopyMatrix(omxMatrix *dest, omxMatrix *orig) {
115         /* Copy a matrix.  NOTE: Matrix maintains its algebra bindings. */
116
117         int regenerateMemory = TRUE;
118
119         if(!dest->owner && (dest->originalRows == orig->rows && dest->originalCols == orig->cols)) {
120                 regenerateMemory = FALSE;                               // If it's local data and the right size, we can keep memory.
121         }
122
123         dest->rows = orig->rows;
124         dest->cols = orig->cols;
125         dest->colMajor = orig->colMajor;
126         dest->originalRows = dest->rows;
127         dest->originalCols = dest->cols;
128         dest->populate = orig->populate;
129
130         if(dest->rows == 0 || dest->cols == 0) {
131                 omxFreeInternalMatrixData(dest);
132                 dest->data = NULL;
133         } else {
134                 if(regenerateMemory) {
135                         omxFreeInternalMatrixData(dest);                                                                                        // Free and regenerate memory
136                         dest->data = (double*) Calloc(dest->rows * dest->cols, double);
137                 }
138                 if (dest->data != orig->data) {  // if equal then programmer Rf_error? TODO
139                         memcpy(dest->data, orig->data, dest->rows * dest->cols * sizeof(double));
140                 }
141         }
142
143         omxMatrixLeadingLagging(dest);
144 }
145
146 void omxFreeMatrix(omxMatrix *om) {
147     
148     if(om == NULL) return;
149
150         omxFreeInternalMatrixData(om);
151
152         if(om->algebra != NULL) {
153                 omxFreeAlgebraArgs(om->algebra);
154                 om->algebra = NULL;
155         }
156
157         if(om->fitFunction != NULL) {
158                 omxFreeFitFunctionArgs(om->fitFunction);
159                 om->fitFunction = NULL;
160         }
161         
162         if (!om->hasMatrixNumber) delete om;
163 }
164
165 /**
166  * Copies an omxMatrix to a new R matrix object
167  *
168  * \param om the omxMatrix to copy
169  * \return a Rf_protect'd SEXP for the R matrix object
170  */
171 SEXP omxExportMatrix(omxMatrix *om) {
172         SEXP nextMat;
173         Rf_protect(nextMat = Rf_allocMatrix(REALSXP, om->rows, om->cols));
174         for(int row = 0; row < om->rows; row++) {
175                 for(int col = 0; col < om->cols; col++) {
176                         REAL(nextMat)[col * om->rows + row] =
177                                 omxMatrixElement(om, row, col);
178                 }
179         }
180         return nextMat;
181 }
182
183 void omxZeroByZeroMatrix(omxMatrix *om) {
184         if (om->rows > 0 || om->cols > 0) {
185                 omxResizeMatrix(om, 0, 0);
186         }
187 }
188
189 omxMatrix* omxNewIdentityMatrix(int nrows, omxState* state) {
190         omxMatrix* newMat = NULL;
191         int l,k;
192
193         newMat = omxInitMatrix(nrows, nrows, TRUE, state);
194         for(k =0; k < newMat->rows; k++) {
195                 for(l = 0; l < newMat->cols; l++) {
196                         if(l == k) {
197                                 omxSetMatrixElement(newMat, k, l, 1);
198                         } else {
199                                 omxSetMatrixElement(newMat, k, l, 0);
200                         }
201                 }
202         }
203         return newMat;
204 }
205
206 omxMatrix* omxDuplicateMatrix(omxMatrix* src, omxState* newState) {
207         omxMatrix* newMat;
208     
209         if(src == NULL) return NULL;
210         newMat = omxInitMatrix(src->rows, src->cols, TRUE, newState);
211         omxCopyMatrix(newMat, src);
212         newMat->hasMatrixNumber = src->hasMatrixNumber;
213         newMat->matrixNumber    = src->matrixNumber;
214         newMat->name = src->name;
215     
216     return newMat;    
217 }
218
219 void omxResizeMatrix(omxMatrix *om, int nrows, int ncols)
220 {
221         // Always Recompute() before you Resize().
222         if(OMX_DEBUG_MATRIX) { 
223                 mxLog("Resizing matrix from (%d, %d) to (%d, %d)",
224                         om->rows, om->cols, nrows, ncols);
225         }
226
227         if( (om->rows != nrows || om->cols != ncols)) {
228                 omxFreeInternalMatrixData(om);
229                 om->data = (double*) Calloc(nrows * ncols, double);
230         }
231
232         om->rows = nrows;
233         om->cols = ncols;
234         om->originalRows = om->rows;
235         om->originalCols = om->cols;
236
237         omxMatrixLeadingLagging(om);
238 }
239
240 double* omxLocationOfMatrixElement(omxMatrix *om, int row, int col) {
241         int index = 0;
242         if(om->colMajor) {
243                 index = col * om->rows + row;
244         } else {
245                 index = row * om->cols + col;
246         }
247         return om->data + index;
248 }
249
250 void vectorElementError(int index, int numrow, int numcol) {
251         char *errstr = (char*) calloc(250, sizeof(char));
252         if ((numrow > 1) && (numcol > 1)) {
253                 sprintf(errstr, "Requested improper index (%d) from a malformed vector of dimensions (%d, %d).", 
254                         index, numrow, numcol);
255         } else {
256                 int Rf_length = (numrow > 1) ? numrow : numcol;
257                 sprintf(errstr, "Requested improper index (%d) from vector of Rf_length (%d).", 
258                         index, Rf_length);
259         }
260         Rf_error(errstr);
261         free(errstr);  // TODO not reached
262 }
263
264 void setMatrixError(omxMatrix *om, int row, int col, int numrow, int numcol) {
265         char *errstr = (char*) calloc(250, sizeof(char));
266         static const char *matrixString = "matrix";
267         static const char *algebraString = "algebra";
268         static const char *fitString = "fit function";
269         const char *typeString;
270         if (om->algebra != NULL) {
271                 typeString = algebraString;
272         } else if (om->fitFunction != NULL) {
273                 typeString = fitString;
274         } else {
275                 typeString = matrixString;
276         }
277         if (om->name == NULL) {
278                 sprintf(errstr, "Attempted to set row and column (%d, %d) in %s with dimensions %d x %d.", 
279                         row, col, typeString, numrow, numcol);
280         } else {
281                 sprintf(errstr, "Attempted to set row and column (%d, %d) in %s \"%s\" with dimensions %d x %d.", 
282                         row, col, typeString, om->name, numrow, numcol);
283         }
284         Rf_error(errstr);
285         free(errstr);  // TODO not reached
286 }
287
288 void matrixElementError(int row, int col, int numrow, int numcol) {
289         Rf_error("Requested improper value (%d, %d) from (%d, %d) matrix", row, col, numrow, numcol);
290 }
291
292 void setVectorError(int index, int numrow, int numcol) {
293         char *errstr = (char*) calloc(250, sizeof(char));
294         if ((numrow > 1) && (numcol > 1)) {
295                 sprintf(errstr, "Attempting to set improper index (%d) from a malformed vector of dimensions (%d, %d).", 
296                         index, numrow, numcol);
297         } else {
298                 int Rf_length = (numrow > 1) ? numrow : numcol;
299                 sprintf(errstr, "Setting improper index (%d) from vector of Rf_length %d.", 
300                         index, Rf_length);
301         }
302         Rf_error(errstr);
303         free(errstr);  // TODO not reached
304 }
305
306 static double omxAliasedMatrixElement(omxMatrix *om, int row, int col)
307 {
308         int index = 0;
309         if(row >= om->originalRows || col >= om->originalCols) {
310                 char *errstr = (char*) calloc(250, sizeof(char));
311                 sprintf(errstr, "Requested improper value (%d, %d) from (%d, %d) matrix.", 
312                         row + 1, col + 1, om->originalRows, om->originalCols);
313                 Rf_error(errstr);
314                 free(errstr);  // TODO not reached
315         return (NA_REAL);
316         }
317         if(om->colMajor) {
318                 index = col * om->originalRows + row;
319         } else {
320                 index = row * om->originalCols + col;
321         }
322         return om->data[index];
323 }
324
325 void omxMarkDirty(omxMatrix *om) { om->version += 1; }
326
327 void omxMarkClean(omxMatrix *om) {
328         om->version += 1;
329         om->cleanVersion = om->version;
330         if (OMX_DEBUG_ALGEBRA) {
331                 const char *name = "?";
332                 if (om->name) name = om->name;
333                 mxLog("Marking matrix %s clean (version %d)", name, om->version);
334         }
335 }
336
337 omxMatrix* omxNewMatrixFromRPrimitive(SEXP rObject, omxState* state, 
338         unsigned short hasMatrixNumber, int matrixNumber) {
339 /* Creates and populates an omxMatrix with details from an R matrix object. */
340         omxMatrix *om = NULL;
341         om = omxInitMatrix(0, 0, TRUE, state);
342         return omxFillMatrixFromRPrimitive(om, rObject, state, hasMatrixNumber, matrixNumber);
343 }
344
345 omxMatrix* omxFillMatrixFromRPrimitive(omxMatrix* om, SEXP rObject, omxState* state,
346         unsigned short hasMatrixNumber, int matrixNumber) {
347 /* Populates the fields of a omxMatrix with details from an R object. */
348         if(rObject && !Rf_isMatrix(rObject) && !Rf_isVector(rObject)) { // Sanity Check
349                 Rf_error("Recieved unknown matrix type in omxFillMatrixFromRPrimitive.");
350         }
351         return(fillMatrixHelperFunction(om, rObject, state, hasMatrixNumber, matrixNumber));
352 }
353
354
355
356 static omxMatrix* fillMatrixHelperFunction(omxMatrix* om, SEXP matrix, omxState* state,
357         unsigned short hasMatrixNumber, int matrixNumber) {
358
359         int* dimList;
360
361         if(OMX_DEBUG) { mxLog("Filling omxMatrix from R matrix."); }
362
363         if (!om) Rf_error("fillMatrixHelperFunction: matrix must be allocated already");
364
365         if (matrix) {
366                 if(Rf_isMatrix(matrix)) {
367                         SEXP matrixDims;
368                         Rf_protect(matrixDims = Rf_getAttrib(matrix, R_DimSymbol));
369                         dimList = INTEGER(matrixDims);
370                         om->rows = dimList[0];
371                         om->cols = dimList[1];
372                         Rf_unprotect(1); // matrixDims
373                 } else if (Rf_isVector(matrix)) {               // If it's a vector, assume it's a row vector. BLAS doesn't care.
374                         if(OMX_DEBUG) { mxLog("Vector discovered.  Assuming rowity."); }
375                         om->rows = 1;
376                         om->cols = Rf_length(matrix);
377                 }
378                 if(OMX_DEBUG) { mxLog("Matrix connected to (%d, %d) matrix or MxMatrix.", om->rows, om->cols); }
379
380                 if (TYPEOF(matrix) != REALSXP) {
381                         // we could avoid a double copy here TODO
382                         SEXP copy;
383                         Rf_protect(copy = Rf_coerceVector(matrix, REALSXP));
384                         om->data = (double*) Realloc(NULL, om->rows * om->cols, double);
385                         memcpy(om->data, REAL(copy), om->rows * om->cols * sizeof(double));
386                 } else {
387                         om->owner = matrix;
388                         om->data = REAL(om->owner);
389                 }
390         }
391
392         om->colMajor = TRUE;
393         om->originalRows = om->rows;
394         om->originalCols = om->cols;
395         om->algebra = NULL;
396         om->fitFunction = NULL;
397         om->currentState = state;
398         om->hasMatrixNumber = hasMatrixNumber;
399         om->matrixNumber = matrixNumber;
400         om->version = 1;
401         omxMarkClean(om);
402
403         omxMatrixLeadingLagging(om);
404
405         return om;
406 }
407
408 void omxMatrix::omxProcessMatrixPopulationList(SEXP matStruct)
409 {
410         if(OMX_DEBUG) { mxLog("Processing Population List: %d elements.", Rf_length(matStruct) - 1); }
411
412         if(Rf_length(matStruct) > 1) {
413                 int numPopLocs = Rf_length(matStruct) - 1;
414                 populate.resize(numPopLocs);
415         }
416
417         for(int i = 0; i < Rf_length(matStruct)-1; i++) {
418                 SEXP subList;
419                 Rf_protect(subList = VECTOR_ELT(matStruct, i+1));
420
421                 int* locations = INTEGER(subList);
422                 populateLocation &pl = populate[i];
423                 pl.from = locations[0];
424                 pl.srcRow = locations[1];
425                 pl.srcCol = locations[2];
426                 pl.destRow = locations[3];
427                 pl.destCol = locations[4];
428                 Rf_unprotect(1); //subList
429         }
430 }
431
432 void omxToggleRowColumnMajor(omxMatrix *mat) {
433
434         int i, j;
435         int nrows = mat->rows;
436         int ncols = mat->cols;
437         
438         double *newdata = (double*) Calloc(nrows * ncols, double);
439         double *olddata = mat->data;
440
441         if (mat->colMajor) {
442                 for(i = 0; i < ncols; i++) {
443                         for(j = 0; j < nrows; j++) {
444                                 newdata[i + ncols * j] = olddata[i * nrows + j];
445                         }
446                 }
447         } else {
448                 for(i = 0; i < nrows; i++) {
449                         for(j = 0; j < ncols; j++) {
450                                 newdata[i + nrows * j] = olddata[i * ncols + j];
451                         }
452                 }
453         }
454
455         omxFreeInternalMatrixData(mat);
456         mat->data = newdata;
457         mat->colMajor = !mat->colMajor;
458 }
459
460 void omxTransposeMatrix(omxMatrix *mat) {
461         mat->colMajor = !mat->colMajor;
462         
463         if(mat->rows != mat->cols){
464         int mid = mat->rows;
465         mat->rows = mat->cols;
466         mat->cols = mid;
467         }
468         
469         omxMatrixLeadingLagging(mat);
470 }
471
472 void omxRemoveElements(omxMatrix *om, int numRemoved, int removed[]) {
473
474         if(numRemoved < 1) { return; }
475
476         int oldElements;
477
478         if (om->rows > 1) {
479                 if(om->originalRows == 0) Rf_error("Not allocated");
480                 oldElements = om->originalRows;
481                 om->rows = oldElements - numRemoved;
482         } else {
483                 if(om->originalCols == 0) Rf_error("Not allocated");
484                 oldElements = om->originalCols;
485                 om->cols = oldElements - numRemoved;
486         }
487
488         int nextElement = 0;
489
490         for(int j = 0; j < oldElements; j++) {
491                 if(!removed[j]) {
492                         omxUnsafeSetVectorElement(om, nextElement, omxUnsafeVectorElement(om, j));
493                         nextElement++;
494                 }
495         }
496
497         omxMatrixLeadingLagging(om);
498 }
499
500 void omxRemoveRowsAndColumns(omxMatrix *om, int numRowsRemoved, int numColsRemoved, int rowsRemoved[], int colsRemoved[])
501 {
502     // TODO: Create short-circuit form of omxRemoveRowsAndCols to remove just rows or just columns.
503
504         if(numRowsRemoved < 1 && numColsRemoved < 1) { return; }
505
506         int oldRows, oldCols;
507
508         if(om->originalRows == 0 || om->originalCols == 0) Rf_error("Not allocated");
509         if (om->rows != om->originalRows || om->cols != om->originalCols) {
510                 // Feasible, but the code is currently not robust to this case
511                 Rf_error("Can only omxRemoveRowsAndColumns once");
512         }
513
514         oldRows = om->originalRows;
515         oldCols = om->originalCols;
516
517         int nextCol = 0;
518         int nextRow = 0;
519
520         om->rows = oldRows - numRowsRemoved;
521         om->cols = oldCols - numColsRemoved;
522
523         if (om->colMajor) {
524                 for(int j = 0; j < oldCols; j++) {
525                         if(colsRemoved[j]) continue;
526                         nextRow = 0;
527                         for(int k = 0; k < oldRows; k++) {
528                                 if(rowsRemoved[k]) continue;
529                                 omxSetMatrixElement(om, nextRow, nextCol, omxAliasedMatrixElement(om, k, j));
530                                 nextRow++;
531                         }
532                         nextCol++;
533                 }
534         } else {
535                 for(int k = 0; k < oldRows; k++) {
536                         if(rowsRemoved[k]) continue;
537                         nextCol = 0;
538                         for(int j = 0; j < oldCols; j++) {
539                                 if(colsRemoved[j]) continue;
540                                 omxSetMatrixElement(om, nextRow, nextCol, omxAliasedMatrixElement(om, k, j));
541                                 nextCol++;
542                         }
543                         nextRow++;
544                 }
545         }
546
547         omxMatrixLeadingLagging(om);
548 }
549
550 /* Function wrappers that switch based on inclusion of algebras */
551 void omxPrint(omxMatrix *source, const char* d) {                                       // Pretty-print a (small) matrix
552     if(source == NULL) mxLog("%s is NULL.", d);
553         else if(source->algebra != NULL) omxAlgebraPrint(source->algebra, d);
554         else if(source->fitFunction != NULL) omxFitFunctionPrint(source->fitFunction, d);
555         else omxPrintMatrix(source, d);
556 }
557
558 bool omxMatrix::omxPopulateSubstitutions()
559 {
560         if (populate.size() == 0) return false;
561         for (size_t pi = 0; pi < populate.size(); pi++) {
562                 populateLocation &pl = populate[pi];
563                 int index = pl.from;
564                 omxMatrix* sourceMatrix;
565                 if (index < 0) {
566                         sourceMatrix = currentState->matrixList[~index];
567                 } else {
568                         sourceMatrix = currentState->algebraList[index];
569                 }
570
571                 omxRecompute(sourceMatrix);
572                 double value = omxMatrixElement(sourceMatrix, pl.srcRow, pl.srcCol);
573                 omxSetMatrixElement(this, pl.destRow, pl.destCol, value);
574         }
575         return true;
576 }
577
578 void omxMatrixLeadingLagging(omxMatrix *om) {
579         om->majority = &(omxMatrixMajorityList[(om->colMajor?1:0)]);
580         om->minority = &(omxMatrixMajorityList[(om->colMajor?0:1)]);
581         om->leading = (om->colMajor?om->rows:om->cols);
582         om->lagging = (om->colMajor?om->cols:om->rows);
583 }
584
585 unsigned short omxNeedsUpdate(omxMatrix *matrix) {
586         bool yes;
587         if (matrix->hasMatrixNumber && omxMatrixIsClean(matrix)) {
588                 yes = FALSE;
589         } else {
590                 yes = TRUE;
591         }
592         if (OMX_DEBUG_ALGEBRA) {
593                 const char *name = "?";
594                 if (matrix->name) name = matrix->name;
595                 mxLog("Matrix %s %s update", name, yes? "needs" : "does not need");
596         }
597         return yes;
598 }
599
600 static void maybeCompute(omxMatrix *matrix, int want)
601 {
602         if(matrix->omxPopulateSubstitutions()) {
603                 // was a simple matrix and we're done
604         } else if(!omxNeedsUpdate(matrix)) /* do nothing */;
605         else if(matrix->algebra != NULL) omxAlgebraRecompute(matrix->algebra);
606         else if(matrix->fitFunction != NULL) {
607                 omxFitFunctionCompute(matrix->fitFunction, want, NULL);
608         }
609 }
610
611 void omxRecompute(omxMatrix *matrix)
612 {
613         maybeCompute(matrix, FF_COMPUTE_FIT);
614 }
615
616 void omxInitialCompute(omxMatrix *matrix)
617 {
618         if(matrix->omxPopulateSubstitutions()) {
619                 // was a simple matrix and we're done
620         } else if(!omxNeedsUpdate(matrix)) /* do nothing */;
621         else if(matrix->algebra != NULL) omxAlgebraInitialCompute(matrix->algebra);
622         else if(matrix->fitFunction != NULL) {
623                 omxFitFunctionCompute(matrix->fitFunction, FF_COMPUTE_INITIAL_FIT, NULL);
624         }
625 }
626
627 void omxForceCompute(omxMatrix *matrix) {
628         if(matrix->omxPopulateSubstitutions()) {
629                 // was a simple matrix and we're done
630         } else if (matrix->algebra != NULL) omxAlgebraForceCompute(matrix->algebra);
631         else if(matrix->fitFunction != NULL) {
632                 omxFitFunctionCompute(matrix->fitFunction, FF_COMPUTE_FIT, NULL);
633         }
634 }
635
636 void omxMatrix::transposePopulate()
637 {
638         for (size_t px=0; px < populate.size(); ++px) populate[px].transpose();
639 }
640
641 /*
642  * omxShallowInverse
643  *                      Calculates the inverse of (I-A) using an n-step Neumann series
644  * Assumes that A reduces to all zeros after numIters steps
645  *
646  * params:
647  * omxMatrix *A                         : The A matrix.  I-A will be inverted.  Size MxM.
648  * omxMatrix *Z                         : On output: Computed (I-A)^-1. MxM.
649  * omxMatrix *Ax                        : Space for computation. MxM.
650  * omxMatrix *I                         : Identity matrix. Will not be changed on exit. MxM.
651  */
652
653 void omxShallowInverse(FitContext *fc, int numIters, omxMatrix* A, omxMatrix* Z, omxMatrix* Ax, omxMatrix* I )
654 {
655         omxMatrix* origZ = Z;
656     double oned = 1, minusOned = -1.0;
657
658         if(numIters == NA_INTEGER) {
659                 if(OMX_DEBUG_ALGEBRA) { mxLog("RAM Algebra (I-A) inversion using standard (general) inversion."); }
660
661                 /* Z = (I-A)^-1 */
662                 if(I->colMajor != A->colMajor) {
663                         omxTransposeMatrix(I);  // transpose I? Hm? TODO
664                 }
665                 omxCopyMatrix(Z, A);
666
667                 omxDGEMM(FALSE, FALSE, oned, I, I, minusOned, Z);
668
669                 Matrix Zmat(Z);
670                 int info = MatrixInvert1(Zmat);
671                 if (info) {
672                         Z->data[0] = nan("singular");
673                         if (fc) fc->recordIterationError("(I-A) is exactly singular (info=%d)", info);
674                         return;
675                 }
676
677                 if(OMX_DEBUG_ALGEBRA) {omxPrint(Z, "Z");}
678
679         } else {
680
681                 if(OMX_DEBUG_ALGEBRA) { mxLog("RAM Algebra (I-A) inversion using optimized expansion."); }
682
683                 /* Taylor Expansion optimized I-A calculation */
684                 if(I->colMajor != A->colMajor) {
685                         omxTransposeMatrix(I);
686                 }
687
688                 if(I->colMajor != Ax->colMajor) {
689                         omxTransposeMatrix(Ax);
690                 }
691
692                 omxCopyMatrix(Z, A);
693
694                 /* Optimized I-A inversion: Z = (I-A)^-1 */
695                 // F77_CALL(omxunsafedgemm)(I->majority, A->majority, &(I->cols), &(I->rows), &(A->rows), &oned, I->data, &(I->cols), I->data, &(I->cols), &oned, Z->data, &(Z->cols));  // Z = I + A = A^0 + A^1
696                 // omxDGEMM(FALSE, FALSE, 1.0, I, I, 1.0, Z); // Z == A + I
697
698                 for(int i = 0; i < A->rows; i++)
699                         omxSetMatrixElement(Z, i, i, 1);
700
701                 for(int i = 1; i <= numIters; i++) { // TODO: Efficiently determine how many times to do this
702                         // The sequence goes like this: (I + A), I + (I + A) * A, I + (I + (I + A) * A) * A, ...
703                         // Which means only one DGEMM per iteration.
704                         if(OMX_DEBUG_ALGEBRA) { mxLog("....RAM: Iteration #%d/%d", i, numIters);}
705                         omxCopyMatrix(Ax, I);
706                         // F77_CALL(omxunsafedgemm)(A->majority, A->majority, &(Z->cols), &(Z->rows), &(A->rows), &oned, Z->data, &(Z->cols), A->data, &(A->cols), &oned, Ax->data, &(Ax->cols));  // Ax = Z %*% A + I
707                         omxDGEMM(FALSE, FALSE, oned, A, Z, oned, Ax);
708                         omxMatrix* m = Z; Z = Ax; Ax = m;       // Juggle to make Z equal to Ax
709                 }
710                 if(origZ != Z) {        // Juggling has caused Ax and Z to swap
711                         omxCopyMatrix(Z, Ax);
712                 }
713         }
714 }
715
716 double omxMaxAbsDiff(omxMatrix *m1, omxMatrix *m2)
717 {
718         if (m1->rows != m2->rows || m1->cols != m2->cols) Rf_error("Matrices are not the same size");
719
720         double mad = 0;
721         int size = m1->rows * m1->cols;
722         for (int dx=0; dx < size; ++dx) {
723                 double mad1 = fabs(m1->data[dx] - m2->data[dx]);
724                 if (mad < mad1) mad = mad1;
725         }
726         return mad;
727 }