Implemnented definition var calculations (still untested). ML Objective function...
[openmx:openmx.git] / src / omxMatrix.c
1 /*
2  *  Copyright 2007-2009 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
30 const char omxMatrixMajorityList[3] = "Tn";             // BLAS Column Majority.
31
32 void omxMatrixPrint(omxMatrix *source, char* header) {
33         int j, k;
34         
35         Rprintf("%s: (%d x %d) [%s-major]\n", header, source->rows, source->cols, (source->colMajor?"col":"row"));
36         if(OMX_DEBUG) {Rprintf("Matrix Printing is at %0x\n", source);}
37         
38         if(source->colMajor) {
39                 for(j = 0; j < source->rows; j++) {
40                         for(k = 0; k < source->cols; k++) {
41                                 Rprintf("\t%3.6f", source->data[k*source->rows+j]);
42                         }
43                         Rprintf("\n");
44                 }
45         } else {
46                 for(j = 0; j < source->cols; j++) {
47                         for(k = 0; k < source->rows; k++) {
48                                 Rprintf("\t%3.6f", source->data[k*source->cols+j]);
49                         }
50                         Rprintf("\n");
51                 }
52         }
53 }
54
55 omxMatrix* omxInitMatrix(omxMatrix* om, int nrows, int ncols, unsigned short isColMajor) {
56         
57         if(om == NULL) om = (omxMatrix*) R_alloc(1, sizeof(omxMatrix));
58         if(OMX_DEBUG) { Rprintf("Initializing 0x%0x to (%d, %d).\n", om, nrows, ncols); }
59
60         om->rows = nrows;
61         om->cols = ncols;
62         om->colMajor = (isColMajor?1:0);
63
64         om->originalRows = om->rows;
65         om->originalCols = om->cols;
66         om->originalColMajor=om->colMajor;
67         
68         if(om->rows == 0 || om->cols == 0) {
69                 om->data = NULL;
70                 om->localData = FALSE;
71         } else {
72                 om->data = (double*) Calloc(nrows * ncols, double);
73                 om->localData = TRUE;
74         }
75
76         om->aliasedPtr = NULL;
77         om->algebra = NULL;
78         om->objective = NULL;
79         
80         omxMatrixCompute(om);
81         
82         return om;
83         
84 }
85
86 void omxCopyMatrix(omxMatrix *dest, omxMatrix *orig) {
87         /* Duplicate a matrix.  NOTE: Matrix maintains its algebra bindings. */
88         
89         if(OMX_DEBUG) { Rprintf("omxCopyMatrix"); }
90         
91         omxFreeMatrixData(dest);
92
93         dest->rows = orig->rows;
94         dest->cols = orig->cols;
95         dest->colMajor = orig->colMajor;
96         dest->originalRows = dest->rows;
97         dest->originalCols = dest->cols;
98         dest->originalColMajor = dest->colMajor;
99
100         if(dest->rows == 0 || dest->cols == 0) {
101                 dest->data = NULL;
102                 dest->localData=FALSE;
103         } else {
104                 dest->data = (double*) Calloc(dest->rows * dest->cols, double);
105                 memcpy(dest->data, orig->data, dest->rows * dest->cols * sizeof(double));
106                 dest->localData = TRUE;
107         }
108
109         dest->aliasedPtr = NULL;
110
111         omxMatrixCompute(dest);
112         
113 }
114
115 void omxAliasMatrix(omxMatrix *dest, omxMatrix *src) {
116         omxCopyMatrix(dest, src);
117         dest->aliasedPtr = src->data;                                           // Interesting Aside: back matrix can change without alias
118         dest->algebra = NULL;
119         dest->objective = NULL;
120 }
121
122 void omxFreeMatrixData(omxMatrix * om) {
123  
124         if(om->localData && om->data != NULL) {
125                 if(OMX_DEBUG) { Rprintf("Freeing 0x%0x. Localdata = %d.\n", om->data, om->localData); }
126                 Free(om->data);
127                 om->data = NULL;
128                 om->localData = FALSE;
129         }
130
131 }
132
133 void omxFreeAllMatrixData(omxMatrix *om) {
134         
135         if(OMX_DEBUG) { Rprintf("Freeing 0x%0x with data = %0x and algebra %0x.\n", om, om->data, om->algebra); }
136         
137         if(om->localData && om->data != NULL) {
138                 Free(om->data);
139                 om->data = NULL;
140                 om->localData = FALSE;
141         }
142         
143         if(om->algebra != NULL) {
144                 omxFreeAlgebraArgs(om->algebra);
145                 om->algebra = NULL;
146         }
147         
148         if(om->objective != NULL) {
149                 omxFreeObjectiveArgs(om->objective);
150                 om->objective = NULL;
151         }
152
153 }
154
155 void omxResizeMatrix(omxMatrix *om, int nrows, int ncols, unsigned short keepMemory) {
156         if(OMX_DEBUG) { Rprintf("Resizing matrix from (%d, %d) to (%d, %d) (keepMemory: %d)", om->rows, om->cols, nrows, ncols, keepMemory); }
157         if(keepMemory == FALSE) { 
158                 if(OMX_DEBUG) { Rprintf(" and regenerating memory to do it"); }
159                 omxFreeMatrixData(om);
160                 om->data = (double*) Calloc(nrows * ncols, double);
161                 om->localData = TRUE;
162         } else if(om->originalRows * om->originalCols < nrows * ncols) {
163                 warning("Upsizing an existing matrix may cause undefined behavior.\n"); // TODO: Define this behavior?
164         }
165
166         if(OMX_DEBUG) { Rprintf(".\n"); }
167         om->rows = nrows;
168         om->cols = ncols;
169         if(keepMemory == FALSE) {
170                 om->originalRows = om->rows;
171                 om->originalCols = om->cols;
172         }
173         
174         omxMatrixCompute(om);
175 }
176
177 void omxResetAliasedMatrix(omxMatrix *om) {
178         om->rows = om->originalRows;
179         om->cols = om->originalCols;
180         om->colMajor = om->originalColMajor;
181         if(om->aliasedPtr != NULL) {
182                 if(OMX_DEBUG) { omxPrintMatrix(om, "I was");}
183                 memcpy(om->data, om->aliasedPtr, om->rows*om->cols*sizeof(double));
184                 if(OMX_DEBUG) { omxPrintMatrix(om, "I am");}
185         }
186         omxMatrixCompute(om);
187 }
188
189 void omxMatrixCompute(omxMatrix *om) {
190         
191         if(OMX_DEBUG) { Rprintf("Matrix compute: 0x%0x.\n", om); }
192         om->majority = &(omxMatrixMajorityList[(om->colMajor?1:0)]);
193         om->minority = &(omxMatrixMajorityList[(om->colMajor?0:1)]);
194         om->leading = (om->colMajor?om->rows:om->cols);
195         om->lagging = (om->colMajor?om->cols:om->rows);
196         om->isDirty = FALSE;
197 }
198
199 double* omxLocationOfMatrixElement(omxMatrix *om, int row, int col) {
200         int index = 0;
201         if(om->colMajor) {
202                 index = col * om->rows + row;
203         } else {
204                 index = row * om->cols + col;
205         }
206         return om->data + index;
207 }
208
209 double omxMatrixElement(omxMatrix *om, int row, int col) {
210         int index = 0;
211         if(om->colMajor) {
212                 index = col * om->rows + row;
213         } else {
214                 index = row * om->cols + col;
215         }
216         return om->data[index];
217 }
218
219 void omxSetMatrixElement(omxMatrix *om, int row, int col, double value) {
220         int index = 0;
221         if(om->colMajor) {
222                 index = col * om->rows + row;
223         } else {
224                 index = row * om->cols + col;
225         }
226         om->data[index] = value;
227 }
228
229 void omxMarkDirty(omxMatrix *om) { om->isDirty = TRUE; }
230
231 unsigned short omxMatrixNeedsUpdate(omxMatrix *matrix) { return matrix->isDirty; };
232
233 omxMatrix* omxNewMatrixFromMxMatrix(SEXP matrix) {
234 /* Populates the fields of a omxMatrix with details from an R Matrix. */ 
235         
236         SEXP className;
237         SEXP matrixDims;
238         int* dimList;
239         
240         omxMatrix *om = NULL;
241         om = omxInitMatrix(NULL, 0, 0, FALSE);
242         
243         if(OMX_DEBUG) { Rprintf("Filling omxMatrix from R matrix.\n"); }
244         
245         if(matrix == NULL) error("Null Matrix detected.\n");
246         
247         /* Sanity Check */
248         if(!isMatrix(matrix) && !isVector(matrix)) {
249                 SEXP values;
250                 int *rowVec, *colVec;
251                 double  *dataVec;
252                 const char *stringName;
253                 if(OMX_DEBUG) {Rprintf("R Matrix is an object of some sort.\n");}
254                 PROTECT(className = getAttrib(matrix, install("class")));
255                 if(strncmp(CHAR(STRING_ELT(className, 0)), "Symm", 2) != 0) { // Should be "Mx"
256                         error("omxMatrix::fillFromMatrix--Passed Non-vector, non-matrix SEXP.\n");
257                 }
258                 stringName = CHAR(STRING_ELT(className, 0));
259                 if(strncmp(stringName, "SymmMatrix", 12) == 0) {
260                         if(OMX_DEBUG) { Rprintf("R matrix is SymmMatrix.  Processing.\n"); }
261                         PROTECT(values = GET_SLOT(matrix, install("values")));
262                         om->rows = INTEGER(GET_SLOT(values, install("nrow")))[0];
263                         om->cols = INTEGER(GET_SLOT(values, install("ncol")))[0];
264                         
265                         om->data = (double*) S_alloc(om->rows * om->cols, sizeof(double));              // We can afford to keep through the whole call
266                         rowVec = INTEGER(GET_SLOT(values, install("rowVector")));
267                         colVec = INTEGER(GET_SLOT(values, install("colVector")));
268                         dataVec = REAL(GET_SLOT(values, install("dataVector")));
269                         for(int j = 0; j < length(GET_SLOT(values, install("dataVector"))); j++) {
270                                 om->data[(rowVec[j]-1) + (colVec[j]-1) * om->rows] = dataVec[j];
271                                 om->data[(rowVec[j]-1) * om->cols + (colVec[j]-1)] = dataVec[j];  // Symmetric, after all.
272                         }
273                         UNPROTECT(1); // values
274                 }
275                 UNPROTECT(1); // className
276         } else {
277                 if(OMX_DEBUG) { Rprintf("R matrix is Mx Matrix.  Processing.\n"); }
278                 
279                 om->data = REAL(matrix);        // TODO: Class-check first?
280                 
281                 if(isMatrix(matrix)) {
282                         PROTECT(matrixDims = getAttrib(matrix, R_DimSymbol));
283                         dimList = INTEGER(matrixDims);
284                         om->rows = dimList[0];
285                         om->cols = dimList[1];
286                         UNPROTECT(1);   // MatrixDims
287                 } else if (isVector(matrix)) {                  // If it's a vector, assume it's a row vector. BLAS doesn't care.
288                         if(OMX_DEBUG) { Rprintf("Vector discovered.  Assuming rowity.\n"); }
289                         om->rows = 1;
290                         om->cols = length(matrix);
291                 }
292                 if(OMX_DEBUG) { Rprintf("Data connected to (%d, %d) matrix.\n", om->rows, om->cols); }
293         }       
294         
295         om->localData = FALSE;
296         om->colMajor = TRUE;
297         om->originalRows = om->rows;
298         om->originalCols = om->cols;
299         om->originalColMajor = TRUE;
300         om->aliasedPtr = om->data;
301         om->algebra = NULL;
302         om->objective = NULL;
303         
304         if(OMX_DEBUG) { Rprintf("Pre-compute call.\n");}
305         omxMatrixCompute(om);
306         if(OMX_DEBUG) { Rprintf("Post-compute call.\n");}
307
308         if(OMX_DEBUG) {
309                 omxMatrixPrint(om, "Finished importing matrix");
310         }
311         
312         return om;
313 }
314
315 void omxRemoveRowsAndColumns(omxMatrix *om, int numRowsRemoved, int numColsRemoved, int rowsRemoved[], int colsRemoved[])
316 {
317         if(OMX_DEBUG) { Rprintf("Removing %d rows and %d columns from 0x%0x.\n", numRowsRemoved, numColsRemoved, om);}
318         
319         if(om->aliasedPtr == NULL) {  // This is meant only for aliased matrices.  Maybe Need a subclass?
320                 error("removeRowsAndColumns intended only for aliased matrices.\n");
321         }
322         
323         if(numRowsRemoved < 1 || numColsRemoved < 1) { return; }
324                 
325         int numCols = 0;
326         int nextCol = 0;
327         int nextRow = 0;
328         int oldRows = om->rows;
329         int oldCols = om->cols;
330         int j,k;
331         
332         om->rows = om->rows - numRowsRemoved;
333         om->cols = om->cols - numColsRemoved;
334         
335         // Note:  This really aught to be done using a matrix multiply.  Why isn't it?
336         if(om->colMajor) {
337                 for(int j = 0; j < oldCols; j++) {
338                         if(OMX_DEBUG) { Rprintf("Handling %d rows.\n", j);}
339                         if(colsRemoved[j]) {
340                                 continue;
341                         } else {
342                                 nextRow = 0;
343                                 for(int k = 0; k <= oldRows; k++) {
344                                         if(rowsRemoved[k]) {
345                                                 continue;
346                                         } else {
347                                                 omxSetMatrixElement(om, nextRow, nextCol, om->aliasedPtr[k + j * oldRows]);
348                                                 nextRow++;
349                                         }
350                                 }
351                                 nextCol++;
352                         }
353                 }
354         } else {
355                 for(int j = 0; j < oldRows; j++) {
356                         if(rowsRemoved[j]) {
357                                 continue;
358                         } else {
359                                 nextCol = 0;
360                                 for(int k = 0; k <= oldCols; k++) {
361                                         if(colsRemoved[k]) {
362                                                 continue;
363                                         } else {
364                                                 omxSetMatrixElement(om, nextRow, nextCol, om->aliasedPtr[k + j * oldCols]);
365                                                 nextCol++;
366                                         }
367                                 }
368                                 nextRow++;
369                         }
370                 }
371         }
372
373         omxMatrixCompute(om);
374 }
375
376 /* Function wrappers that switch based on inclusion of algebras */
377 void omxPrintMatrix(omxMatrix *source, char* d) {                                       // Pretty-print a (small) matrix
378         if(source->algebra != NULL) omxAlgebraPrint(source->algebra, d);
379         else if(source->objective != NULL) omxObjectivePrint(source->objective, d);
380         else omxMatrixPrint(source, d);
381 }
382
383 unsigned short inline omxNeedsUpdate(omxMatrix *matrix) {
384         if(OMX_DEBUG) {Rprintf("MatrixNeedsUpdate?");}
385         if(matrix->isDirty) return TRUE;
386         if(matrix->algebra != NULL) return omxAlgebraNeedsUpdate(matrix->algebra); 
387         else if(matrix->objective != NULL) return omxObjectiveNeedsUpdate(matrix->objective);
388         else return omxMatrixNeedsUpdate(matrix);
389 }
390
391 void inline omxRecomputeMatrix(omxMatrix *matrix) {
392         if(!omxNeedsUpdate(matrix)) return;
393         if(matrix->algebra != NULL) omxAlgebraCompute(matrix->algebra);
394         else if(matrix->objective != NULL) omxObjectiveCompute(matrix->objective);
395         else omxMatrixCompute(matrix);
396 }
397
398 void inline omxComputeMatrix(omxMatrix *matrix) {
399         if(matrix->algebra != NULL) omxAlgebraCompute(matrix->algebra);
400         else if(matrix->objective != NULL) omxObjectiveCompute(matrix->objective);
401         else omxMatrixCompute(matrix);
402 }