Enable R_NO_REMAP for a cleaner namespace
[openmx:openmx.git] / src / omxAlgebraFunctions.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 *  omxAlgebraFunctions.c
20 *
21 *  Created: Timothy R. Brick    Date: 2008-11-13 12:33:06
22 *
23 *       Includes the functions required for omxAlgebra statements.
24 *   These functions should take a number of values that
25 *   evenly matches the number of args requested by the
26 *   omxSymbolTable.
27 *
28 **********************************************************/
29
30 #include "omxAlgebraFunctions.h"
31 #include "omxMatrix.h"
32 #include "merge.h"
33 #include "omxBLAS.h"
34 #include "omxOpenmpWrap.h"
35 #include "omxSadmvnWrapper.h"
36 #include "matrix.h"
37
38 void omxStandardizeCovMatrix(omxMatrix* cov, double* corList, double* weights) {
39         // Maybe coerce this into an algebra or sequence of algebras?
40
41         if(OMX_DEBUG) { mxLog("Standardizing matrix."); }
42
43         int rows = cov->rows;
44
45         for(int i = 0; i < rows; i++) {
46                 weights[i] = sqrt(omxMatrixElement(cov, i, i));
47         }
48
49         for(int i = 0; i < rows; i++) {
50                 for(int j = 0; j < i; j++) {
51                         corList[((i*(i-1))/2) + j] = omxMatrixElement(cov, i, j) / (weights[i] * weights[j]);
52                 }
53         }
54 }
55
56 void checkIncreasing(omxMatrix* om, int column) {
57         double previous = - INFINITY;
58         double current;
59         for(int j = 0; j < om->rows; j++ ) {
60                 current = omxMatrixElement(om, j, column);
61                 if(isnan(current) || current == NA_INTEGER) {
62                         continue;
63                 }
64                 if(current <= previous) {
65                         char *errstr = (char*) calloc(250, sizeof(char));
66                         sprintf(errstr, "Thresholds are not strictly increasing.");
67                         //TODO: Count 'em all, then throw an Rf_error that lists which ones.
68                         omxRaiseError(om->currentState, -1, errstr);
69                         free(errstr);
70                 }
71         }
72 }
73
74
75
76 // TODO: Implement wrappers for BLAS functions used here.
77
78 /* omxAlgebraFunction Wrappers */
79
80 void omxMatrixTranspose(omxMatrix** matList, int numArgs, omxMatrix* result) {
81
82         omxMatrix* inMat = matList[0];
83
84         omxCopyMatrix(result, inMat);
85         result->colMajor = !result->colMajor;
86         int rowtemp = result->rows;
87         result->rows = result->cols;
88         result->cols = rowtemp;
89         int *populateTemp = result->populateToCol;
90         result->populateToCol = result->populateToRow;
91         result->populateToRow = populateTemp;
92         omxMatrixLeadingLagging(result);
93 }
94
95 void omxMatrixInvert(omxMatrix** matList, int numArgs, omxMatrix* result)
96 {
97         omxMatrix* inMat = matList[0];
98         omxCopyMatrix(result, inMat);
99
100         Matrix resultMat(result);
101         int info = MatrixInvert1(result);
102         if (info) {
103                 omxRaiseErrorf(result->currentState, "(I-A) is exactly singular (info=%d)", info);
104         }
105 }
106
107 static bool isElemConformable(const char *op, omxMatrix *mat1, omxMatrix *mat2)
108 {
109         if (mat1->cols == mat2->cols && mat1->rows == mat2->rows) return true;
110
111         omxRaiseErrorf(mat1->currentState,
112                        "Non-conformable matrices in %s; rows %d != %d or cols %d != %d",
113                        op, mat1->rows, mat2->rows, mat1->cols, mat2->cols);
114
115         return false;
116 }
117
118 void omxMatrixMult(omxMatrix** matList, int numArgs, omxMatrix* result)
119 {
120         omxMatrix* preMul = matList[0];
121         omxMatrix* postMul = matList[1];
122
123         if(preMul == NULL || postMul == NULL) {
124                 char *errstr = (char*) calloc(250, sizeof(char));
125                 sprintf(errstr, "Null matrix pointer detected.\n");
126                 free(errstr);
127                 return;
128         }
129
130         /* Conformability Check! */
131         if(preMul->cols != postMul->rows) {
132                 char *errstr = (char*) calloc(250, sizeof(char));
133                 sprintf(errstr, "Non-conformable matrices [(%d x %d) and (%d x %d)] in Matrix Multiply.", preMul->rows, preMul->cols, postMul->rows, postMul->cols);
134                 omxRaiseError(result->currentState, -1, errstr);
135                 free(errstr);
136                 return;
137         }
138
139         if(result->rows != preMul->rows || result->cols != postMul->cols)
140                 omxResizeMatrix(result, preMul->rows, postMul->cols, FALSE);
141
142         omxDGEMM(FALSE, FALSE, 1.0, preMul, postMul, 0.0, result);
143
144         result->colMajor = TRUE;
145
146         omxMatrixLeadingLagging(result);
147 }
148
149 void omxElementPower(omxMatrix** matList, int numArgs, omxMatrix* result)
150 {
151         omxMatrix* first = matList[0];
152         omxMatrix* second = matList[1];
153
154         if (!isElemConformable("element power", first, second)) return;
155
156         int rows = first->rows;
157         int cols = first->cols;
158         int size = rows * cols;
159
160         if((rows != result->rows) || (cols != result->cols)) {
161                 omxResizeMatrix(result, rows, cols, FALSE);
162         }
163         
164         if (first->colMajor == second->colMajor) {
165                 for(int i = 0; i < size; i++) {
166                         omxSetVectorElement(result, i,
167                                 pow(omxVectorElement(first, i),
168                                         omxVectorElement(second, i)));
169                 }
170                 result->colMajor = first->colMajor;
171                 omxMatrixLeadingLagging(result);
172         } else {
173                 for(int i = 0; i < rows; i++) {
174                         for(int j = 0; j < cols; j++) {
175                                 omxSetMatrixElement(result, i, j,
176                                         pow(omxMatrixElement(first, i, j),
177                                                 omxMatrixElement(second, i, j)));
178                         }
179                 }
180         }
181 }
182
183 void omxMatrixElementMult(omxMatrix** matList, int numArgs, omxMatrix* result)
184 {
185         omxMatrix* first = matList[0];
186         omxMatrix* second = matList[1];
187
188         if (!isElemConformable("element multiplication", first, second)) return;
189
190         int rows = first->rows;
191         int cols = first->cols;
192         int size = rows * cols;
193
194         if((rows != result->rows) || (cols != result->cols)) {
195                 omxResizeMatrix(result, rows, cols, FALSE);
196         }
197         
198         if (first->colMajor == second->colMajor) {
199                 for(int i = 0; i < size; i++) {
200                         omxSetVectorElement(result, i,
201                                 omxVectorElement(first, i) *
202                                 omxVectorElement(second, i));
203                 }
204                 result->colMajor = first->colMajor;
205                 omxMatrixLeadingLagging(result);
206         } else {
207                 for(int i = 0; i < rows; i++) {
208                         for(int j = 0; j < cols; j++) {
209                                 omxSetMatrixElement(result, i, j,
210                                         omxMatrixElement(first, i, j) *
211                                         omxMatrixElement(second, i, j));
212                         }
213                 }
214         }
215 }
216
217
218 void omxKroneckerProd(omxMatrix** matList, int numArgs, omxMatrix* result)
219 {
220         omxMatrix* preMul = matList[0];
221         omxMatrix* postMul = matList[1];
222
223         int preMulRows = preMul->rows;
224         int preMulCols = preMul->cols;
225         int postMulRows = postMul->rows;
226         int postMulCols = postMul->cols;
227         int rows = preMulRows * postMulRows;
228         int cols = preMulCols * postMulCols;
229
230         if(result->rows != rows || result->cols != cols)
231                 omxResizeMatrix(result, rows, cols, FALSE);
232
233         for(int preRow = 0; preRow < preMulRows; preRow++)
234                 for(int postRow = 0; postRow < postMulRows; postRow++)
235                         for(int preCol = 0; preCol < preMulCols; preCol++)
236                                 for(int postCol = 0; postCol < postMulCols; postCol++)
237                                         omxSetMatrixElement(result, preRow * postMulRows + postRow,
238                                                 preCol * postMulCols + postCol,
239                                                 omxMatrixElement(preMul, preRow, preCol) * omxMatrixElement(postMul, postRow, postCol));
240 }
241
242 void omxKroneckerPower(omxMatrix** matList, int numArgs, omxMatrix* result)
243 {
244         omxMatrix* preMul = matList[0];
245         omxMatrix* postMul = matList[1];
246
247         int rows = preMul->rows * postMul->rows;
248         int cols = preMul->cols * postMul->cols;
249
250         if(result->rows != rows || result->cols != cols)
251                 omxResizeMatrix(result, rows, cols, FALSE);
252
253         for(int preRow = 0; preRow < preMul->rows; preRow++)
254                 for(int postRow = 0; postRow < postMul->rows; postRow++)
255                         for(int preCol = 0; preCol < preMul->cols; preCol++)
256                                 for(int postCol = 0; postCol < postMul->cols; postCol++)
257                                         omxSetMatrixElement(result, preRow*postMul->rows + postRow,
258                                                 preCol*postMul->cols + postCol,
259                                                 pow(omxMatrixElement(preMul, preRow, preCol), omxMatrixElement(postMul, postRow, postCol)));
260 }
261
262 void omxQuadraticProd(omxMatrix** matList, int numArgs, omxMatrix* result)
263 {
264         omxMatrix* preMul = matList[0];
265         omxMatrix* postMul = matList[1];
266         /* A %&% B = ABA' */
267
268         static double zero = 0.0;
269         static double one = 1.0;
270
271         /* Conformability Check! */
272         if(preMul->cols != postMul->rows || postMul->rows != postMul->cols) {
273                 omxRaiseError(preMul->currentState, -1, "Non-conformable matrices in Matrix Quadratic Product.");
274                 return;
275         }
276
277         omxMatrix* intermediate = NULL;
278         intermediate = omxInitTemporaryMatrix(NULL, preMul->rows, postMul->cols, TRUE, preMul->currentState);
279
280         if(OMX_DEBUG_ALGEBRA) { mxLog("Quadratic: step = %ld.", intermediate->currentState->computeCount);}
281
282         if(result->rows != preMul->rows || result->cols != preMul->rows)
283                 omxResizeMatrix(result, preMul->rows, preMul->rows, FALSE);
284
285         /* The call itself */
286         if(OMX_DEBUG_ALGEBRA) { mxLog("Quadratic: premul.");}
287         F77_CALL(omxunsafedgemm)((preMul->majority), (postMul->majority), &(preMul->rows), &(postMul->cols), &(preMul->cols), &one, preMul->data, &(preMul->leading), postMul->data, &(postMul->leading), &zero, intermediate->data, &(intermediate->leading));
288
289         if(OMX_DEBUG_ALGEBRA) { mxLog("Quadratic: postmul.");}
290 //      if(OMX_DEBUG_ALGEBRA) { mxLog("Quadratic postmul: result is (%d x %d), %d leading, inter is (%d x %d), prem is (%d x %d), post is (%d x %d).", result->rows, result->cols, result->leading, intermediate->rows, intermediate->cols, preMul->rows, preMul->cols, postMul->rows, postMul->cols);}
291         F77_CALL(omxunsafedgemm)((intermediate->majority), (preMul->minority), &(intermediate->rows), &(preMul->rows), &(intermediate->cols), &one, intermediate->data, &(intermediate->leading), preMul->data, &(preMul->leading), &zero, result->data, &(result->leading));
292         if(OMX_DEBUG_ALGEBRA) { mxLog("Quadratic: clear.");}
293
294         omxFreeAllMatrixData(intermediate);
295
296 }
297
298 void omxElementDivide(omxMatrix** matList, int numArgs, omxMatrix* result)
299 {
300         omxMatrix* first = matList[0];
301         omxMatrix* second = matList[1];
302
303         if (!isElemConformable("element divide", first, second)) return;
304
305         int rows = first->rows;
306         int cols = first->cols;
307         int size = rows * cols;
308
309         if((rows != result->rows) || (cols != result->cols)) {
310                 omxResizeMatrix(result, rows, cols, FALSE);
311         }
312         
313         if (first->colMajor == second->colMajor) {
314                 for(int i = 0; i < size; i++) {
315                         omxSetVectorElement(result, i,
316                                 omxVectorElement(first, i) /
317                                 omxVectorElement(second, i));
318                 }
319                 result->colMajor = first->colMajor;
320                 omxMatrixLeadingLagging(result);
321         } else {
322                 for(int i = 0; i < rows; i++) {
323                         for(int j = 0; j < cols; j++) {
324                                 omxSetMatrixElement(result, i, j,
325                                         omxMatrixElement(first, i, j) /
326                                         omxMatrixElement(second, i, j));
327                         }
328                 }
329         }
330 }
331
332 void omxUnaryNegation(omxMatrix** matList, int numArgs, omxMatrix* result)
333 {
334         omxMatrix* inMat = matList[0];
335
336         int rows = inMat->rows;
337         int cols = inMat->cols;
338
339         if((rows != result->rows) || (cols != result->cols)){
340                 omxResizeMatrix(result, rows, cols, FALSE);
341         }
342
343         int vec_Rf_length = rows * cols;
344         for (int i=0; i < vec_Rf_length; i++){
345                 double ith_value = omxVectorElement(inMat, i);
346                 if (ith_value == 0.0){
347                         omxSetVectorElement(result, i, 1.0);
348                 }
349                 else {
350                         omxSetVectorElement(result, i, 0.0);
351                 }
352         }
353         result->colMajor = inMat->colMajor;
354         omxMatrixLeadingLagging(result);
355 }
356
357 void omxBinaryOr(omxMatrix** matList, int numArgs, omxMatrix* result)
358 {
359         omxMatrix* first = matList[0];
360         omxMatrix* second = matList[1];
361
362         if (!isElemConformable("binary or", first, second)) return;
363
364                 int rows = first->rows;
365                 int cols = first->cols;
366                 int size = rows * cols;
367
368             if((rows != result->rows) || (cols != result->cols)){
369                         omxResizeMatrix(result, rows, cols, FALSE);
370             }
371
372                 if (first->colMajor == second->colMajor) {
373                         for(int i = 0; i < size; i++) {
374                                         double ith_first  = omxVectorElement(first, i);
375                                         double ith_second =omxVectorElement(second, i);
376                                         if ((ith_first == 0.0) && (ith_second == 0.0)){
377                                                 omxSetVectorElement(result, i, 0.0);
378                                         }
379                                         else {
380                                                 omxSetVectorElement(result, i, 1.0);
381                                         }
382                         }
383                 result->colMajor = first->colMajor;
384                 omxMatrixLeadingLagging(result);
385                 } else {
386                         for(int i = 0; i < rows; i++) {
387                                 for(int j = 0; j < cols; j++) {
388                                                         double ith_first  = omxMatrixElement(first, i, j);
389                                                         double ith_second = omxMatrixElement(second, i, j);
390                                         if ((ith_first == 0.0) && (ith_second == 0.0)){
391                                                 omxSetMatrixElement(result, i, j, 0.0);
392                                         }
393                                         else {
394                                                 omxSetMatrixElement(result, i, j, 1.0);
395                                         }
396                                 }
397                         }
398                 }
399 }
400
401 void omxBinaryAnd(omxMatrix** matList, int numArgs, omxMatrix* result){
402                 omxMatrix* first = matList[0];
403                     omxMatrix* second = matList[1];
404
405         if (!isElemConformable("binary and", first, second)) return;
406
407                 int rows = first->rows;
408                 int cols = first->cols;
409                 int size = rows * cols;
410
411             if((rows != result->rows) || (cols != result->cols)){
412                      omxResizeMatrix(result, rows, cols, FALSE);
413             }
414
415                 if (first->colMajor == second->colMajor) {
416                         for(int i = 0; i < size; i++) {
417                                         double ith_first  = omxVectorElement(first, i);
418                                         double ith_second =omxVectorElement(second, i);
419                                         if ((ith_first == 0.0) || (ith_second == 0.0)){
420                                                 omxSetVectorElement(result, i, 0.0);
421                                         }
422                                         else {
423                                                 omxSetVectorElement(result, i, 1.0);
424                                         }
425                         }
426                 result->colMajor = first->colMajor;
427                 omxMatrixLeadingLagging(result);
428                 } else {
429                         for(int i = 0; i < rows; i++) {
430                                 for(int j = 0; j < cols; j++) {
431                                                         double ith_first  = omxMatrixElement(first, i, j);
432                                                         double ith_second = omxMatrixElement(second, i, j);
433                                         if ((ith_first == 0.0) || (ith_second == 0.0)){
434                                                 omxSetMatrixElement(result, i, j, 0.0);
435                                         }
436                                         else {
437                                                 omxSetMatrixElement(result, i, j, 1.0);
438                                         }
439                                 }
440                         }
441                 }
442 }
443
444 void omxBinaryLessThan(omxMatrix** matList, int numArgs, omxMatrix* result){
445                 omxMatrix* first = matList[0];
446                     omxMatrix* second = matList[1];
447
448         if (!isElemConformable("binary less than", first, second)) return;
449
450                 int rows = first->rows;
451                 int cols = first->cols;
452                 int size = rows * cols;
453
454             if((rows != result->rows) || (cols != result->cols)){
455                      omxResizeMatrix(result, rows, cols, FALSE);
456             }
457
458                 if (first->colMajor == second->colMajor) {
459                         for(int i = 0; i < size; i++) {
460                                 double ith_value = omxVectorElement(first, i) -
461                                                    omxVectorElement(second, i);
462                                                 if (ith_value < 0.0){
463                                                         omxSetVectorElement(result, i, 1.0);
464                                                 }
465                                                 else {
466                                                         omxSetVectorElement(result, i, 0.0);
467                                                 }
468                         }
469                 result->colMajor = first->colMajor;
470                 omxMatrixLeadingLagging(result);
471                 } else {
472                         for(int i = 0; i < rows; i++) {
473                                 for(int j = 0; j < cols; j++) {
474                                         double ith_value = omxMatrixElement(first, i, j) -
475                                            omxMatrixElement(second, i, j);
476
477                                         if (ith_value < 0.0){
478                                                 omxSetMatrixElement(result, i, j, 1.0);
479                                         }
480                                         else {
481                                                 omxSetMatrixElement(result, i, j, 0.0);
482                                         }
483                                 }
484                         }
485                 }
486 }
487
488 void omxBinaryGreaterThan(omxMatrix** matList, int numArgs, omxMatrix* result)
489 {
490         omxMatrix* first = matList[0];
491             omxMatrix* second = matList[1];
492
493         if (!isElemConformable("binary greater than", first, second)) return;
494
495         int rows = first->rows;
496         int cols = first->cols;
497         int size = rows * cols;
498
499         if((rows != result->rows) || (cols != result->cols)){
500                 omxResizeMatrix(result, rows, cols, FALSE);
501         }
502
503         if (first->colMajor == second->colMajor) {
504                 for(int i = 0; i < size; i++) {
505                         double ith_value = omxVectorElement(first, i) -
506                                            omxVectorElement(second, i);
507                         if (ith_value > 0.0){
508                                 omxSetVectorElement(result, i, 1.0);
509                         }
510                         else {
511                                 omxSetVectorElement(result, i, 0.0);
512                         }
513                 }
514         result->colMajor = first->colMajor;
515         omxMatrixLeadingLagging(result);
516         } else {
517                 for(int i = 0; i < rows; i++) {
518                         for(int j = 0; j < cols; j++) {
519                                 double ith_value = omxMatrixElement(first, i, j) -
520                                                    omxMatrixElement(second, i, j);
521
522                                 if (ith_value > 0.0){
523                                         omxSetMatrixElement(result, i, j, 1.0);
524                                 }
525                                 else {
526                                         omxSetMatrixElement(result, i, j, 0.0);
527                                 }
528                         }
529                 }
530         }
531 }
532
533 void omxBinaryApproxEquals(omxMatrix** matList, int numArgs, omxMatrix* result)
534 {
535         omxMatrix* first  = matList[0];
536             omxMatrix* second = matList[1];
537                 omxMatrix* epsilon = matList[2]; 
538                 
539         if (!isElemConformable("binary approx equals", first, second)) return;
540         if (!isElemConformable("binary approx equals", first, epsilon)) return;
541
542         int rows = first->rows;
543         int cols = first->cols;
544         int size = rows * cols;
545         double negativeOne = -1.0;
546
547     if((rows != result->rows) || (cols != result->cols)){
548                 omxResizeMatrix(result, rows, cols, FALSE);
549     }
550
551         if (first->colMajor == second->colMajor && second->colMajor == epsilon->colMajor) {
552                 for(int i = 0; i < size; i++) {
553                 double ith_value = omxVectorElement(first, i) -
554                                            omxVectorElement(second, i);
555                                 double epsilon_value = omxVectorElement(epsilon, i);
556                                 
557                                 if (ith_value < 0.0){
558                                         ith_value = ith_value * negativeOne;
559                                 }
560                                 if (ith_value < epsilon_value){
561                                         omxSetVectorElement(result, i, 1.0);
562                                 }
563                                 else {
564                                         omxSetVectorElement(result, i, 0.0);
565                                 }
566                 }
567         result->colMajor = first->colMajor;
568         omxMatrixLeadingLagging(result);
569         } else {
570                 for(int i = 0; i < rows; i++) {
571                         for(int j = 0; j < cols; j++) {
572                                                     double ith_value = omxMatrixElement(first, i, j) -
573                                                    omxMatrixElement(second, i, j);
574
575                                                         double epsilon_value = omxMatrixElement(epsilon, i, j);
576                                                         if (ith_value < 0.0){
577                                                                 ith_value = ith_value * negativeOne;
578                                                         }
579                                 if (ith_value < epsilon_value){
580                                         omxSetMatrixElement(result, i, j, 1.0);
581                                 }
582                                 else {
583                                         omxSetMatrixElement(result, i, j, 0.0);
584                                 }
585                         }
586                 }
587         }
588
589 }
590
591 void omxMatrixAdd(omxMatrix** matList, int numArgs, omxMatrix* result)
592 {
593         omxMatrix* first = matList[0];
594         omxMatrix* second = matList[1];
595
596         if (!isElemConformable("matrix add", first, second)) return;
597
598         int rows = first->rows;
599         int cols = first->cols;
600         int size = rows * cols;
601
602         if((rows != result->rows) || (cols != result->cols)) {
603                 omxResizeMatrix(result, rows, cols, FALSE);
604         }
605         
606         if (first->colMajor == second->colMajor) {
607                 for(int i = 0; i < size; i++) {
608                         omxSetVectorElement(result, i,
609                                 omxVectorElement(first, i) +
610                                 omxVectorElement(second, i));
611                 }
612                 result->colMajor = first->colMajor;
613                 omxMatrixLeadingLagging(result);
614         } else {
615                 for(int i = 0; i < rows; i++) {
616                         for(int j = 0; j < cols; j++) {
617                                 omxSetMatrixElement(result, i, j,
618                                         omxMatrixElement(first, i, j) +
619                                         omxMatrixElement(second, i, j));
620                         }
621                 }
622         }
623 }
624
625 int matrixExtractIndices(omxMatrix *source, int dimLength, int **indices, omxMatrix *result) {
626
627         int *retval;
628         /* Case 1: the source vector contains no elements */
629         if (source->rows == 0 || source->cols == 0) {
630                 retval = (int*) calloc(dimLength, sizeof(int));
631                 for(int i = 0; i < dimLength; i++) {
632                         retval[i] = i;
633                 }
634                 *indices = retval;
635                 return(dimLength);
636         }
637         int zero = 0, positive = 0, negative = 0;
638         /* Count the number of zero, positive, and negative elements */
639         for(int i = 0; i < source->rows * source->cols; i++) {
640                 double delement = omxVectorElement(source, i);
641                 if (!R_finite(delement)) {
642                         char *errstr = (char*) calloc(250, sizeof(char));
643                         sprintf(errstr, "non-finite value in '[' operator.\n");
644                         omxRaiseError(result->currentState, -1, errstr);
645                         free(errstr);
646                         return(0);
647                 }
648                 int element = (int) delement;
649                 if (element < 0) {
650                         /* bounds checking */
651                         if (element < - dimLength) {
652                                 char *errstr = (char*) calloc(250, sizeof(char));
653                                 sprintf(errstr, "index %d is out of bounds in '[' operator.", element);
654                                 omxRaiseError(result->currentState, -1, errstr);
655                                 free(errstr);
656                                 return(0);
657                         }
658                         negative++;
659                 } else if (element == 0) {
660                         zero++;
661                 } else {
662                         /* bounds checking */
663                         if (element > dimLength) {
664                                 char *errstr = (char*) calloc(250, sizeof(char));
665                                 sprintf(errstr, "index %d is out of bounds in '[' operator.", element);
666                                 omxRaiseError(result->currentState, -1, errstr);
667                                 free(errstr);
668                                 return(0);
669                         }
670                         positive++;
671                 }
672         }
673         /* It is illegal to mix positive and negative elements */
674         if (positive > 0 && negative > 0) {
675                 char *errstr = (char*) calloc(250, sizeof(char));
676                 sprintf(errstr, "Positive and negative indices together in '[' operator.");
677                 omxRaiseError(result->currentState, -1, errstr);
678                 free(errstr);
679                 return(0);
680         }
681         /* convert negative indices into a list of positive indices */
682         if (negative > 0) {
683                 int *track = (int*) calloc(dimLength, sizeof(int));
684                 int Rf_length = dimLength;
685                 for(int i = 0; i < source->rows * source->cols; i++) {
686                         int element = (int) omxVectorElement(source, i);
687                         if (element < 0) {
688                                 if (!track[-element - 1]) Rf_length--;
689                                 track[-element - 1]++;
690                         }
691                 }
692                 if (Rf_length == 0) {
693                         free(track);
694                         return(0);
695                 }
696                 retval = (int*) calloc(Rf_length, sizeof(int));
697                 int j = 0;
698                 for(int i = 0; i < dimLength; i++) {
699                         if(!track[i]) {
700                                 retval[j++] = i;
701                         }
702                 }
703                 free(track);
704                 *indices = retval;
705                 return(Rf_length);
706         }
707         /* convert positive indices with offset of zero instead of one */
708         if (positive > 0) {
709                 int Rf_length = positive - zero;
710                 retval = (int*) calloc(Rf_length, sizeof(int));
711                 int j = 0;
712                 for(int i = 0; i < source->rows * source->cols; i++) {
713                         int element = (int) omxVectorElement(source, i);
714                         if (element > 0) {
715                                 retval[j++] = element - 1;
716                         }
717                 }
718                 *indices = retval;
719                 return(Rf_length);
720         }
721         /* return zero Rf_length if no positive or negative elements */
722         return(0);
723 }
724
725 void omxMatrixExtract(omxMatrix** matList, int numArgs, omxMatrix* result)
726 {
727         omxMatrix* inMat = matList[0];
728         omxMatrix* rowMatrix = matList[1];
729         omxMatrix* colMatrix = matList[2];
730
731         if(OMX_DEBUG_ALGEBRA) { omxPrint(rowMatrix, "Row matrix: "); }
732         if(OMX_DEBUG_ALGEBRA) { omxPrint(colMatrix, "Col matrix: "); }
733
734         int *rowIndices, *colIndices;
735         int rowIndexLength, colIndexLength;
736
737         rowIndexLength = matrixExtractIndices(rowMatrix, inMat->rows, &rowIndices, result);
738         colIndexLength = matrixExtractIndices(colMatrix, inMat->cols, &colIndices, result);
739
740         if (result->rows != rowIndexLength || result->cols != colIndexLength) {
741                 omxResizeMatrix(result, rowIndexLength, colIndexLength, FALSE);
742         }
743
744         for(int row = 0; row < rowIndexLength; row++) {
745                 for(int col = 0; col < colIndexLength; col++) {
746                         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: Matrix Extract: (%d, %d)[%d, %d] <- (%d, %d)[%d,%d].", result->rows, result->cols, row, col, rowIndexLength, colIndexLength, rowIndices[row], colIndices[col]);}
747                         double element = omxMatrixElement(inMat, rowIndices[row], colIndices[col]);
748                         omxSetMatrixElement(result, row, col, element);
749                 }
750         }
751
752         if (rowIndexLength > 0) free(rowIndices);
753         if (colIndexLength > 0) free(colIndices);
754
755 }
756
757 void omxMatrixSubtract(omxMatrix** matList, int numArgs, omxMatrix* result)
758 {
759         omxMatrix* first = matList[0];
760         omxMatrix* second = matList[1];
761
762         if (!isElemConformable("matrix subtract", first, second)) return;
763
764         int rows = first->rows;
765         int cols = first->cols;
766         int size = rows * cols;
767
768         if((rows != result->rows) || (cols != result->cols)) {
769                 omxResizeMatrix(result, rows, cols, FALSE);
770         }
771         
772         if (first->colMajor == second->colMajor) {
773                 for(int i = 0; i < size; i++) {
774                         omxSetVectorElement(result, i,
775                                 omxVectorElement(first, i) -
776                                 omxVectorElement(second, i));
777                 }
778                 result->colMajor = first->colMajor;
779                 omxMatrixLeadingLagging(result);
780         } else {
781                 for(int i = 0; i < rows; i++) {
782                         for(int j = 0; j < cols; j++) {
783                                 omxSetMatrixElement(result, i, j,
784                                         omxMatrixElement(first, i, j) -
785                                         omxMatrixElement(second, i, j));
786                         }
787                 }
788         }
789 }
790
791 void omxUnaryMinus(omxMatrix** matList, int numArgs, omxMatrix* result)
792 {
793         omxMatrix* inMat = matList[0];
794
795         int rows = inMat->rows;
796         int cols = inMat->cols;
797         int size = rows * cols;
798
799         if((rows != result->rows) || (cols != result->cols)) {
800                 omxResizeMatrix(result, rows, cols, FALSE);
801         }
802
803         for(int i = 0; i < size; i++) {
804                 omxSetVectorElement(result, i,
805                         - omxVectorElement(inMat, i));
806         }
807         result->colMajor = inMat->colMajor;
808         omxMatrixLeadingLagging(result);
809
810 }
811
812 void omxMatrixHorizCat(omxMatrix** matList, int numArgs, omxMatrix* result)
813 {
814         int totalRows = 0, totalCols = 0, currentCol=0;
815
816         if(numArgs == 0) return;
817
818         totalRows = matList[0]->rows;                   // Assumed constant.  Assert this below.
819
820         for(int j = 0; j < numArgs; j++) {
821                 if(totalRows != matList[j]->rows) {
822                         char *errstr = (char*) calloc(250, sizeof(char));
823                         sprintf(errstr, "Non-conformable matrices in horizontal concatenation (cbind). First argument has %d rows, and argument #%d has %d rows.", totalRows, j + 1, matList[j]->rows);
824                         omxRaiseError(result->currentState, -1, errstr);
825                         free(errstr);
826                         return;
827                 }
828                 totalCols += matList[j]->cols;
829         }
830
831         if(result->rows != totalRows || result->cols != totalCols) {
832                 if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: HorizCat: resizing result.");}
833                 omxResizeMatrix(result, totalRows, totalCols, FALSE);
834         }
835
836         int allArgumentsColMajor = result->colMajor;
837         for(int j = 0; j < numArgs && allArgumentsColMajor; j++) {
838                 if (!matList[j]->colMajor) allArgumentsColMajor = 0;
839         }
840
841         if (allArgumentsColMajor) {
842                 int offset = 0;
843                 for(int j = 0; j < numArgs; j++) {      
844                         omxMatrix* current = matList[j];
845                         int size = current->rows * current->cols;
846                         memcpy(result->data + offset, current->data, size * sizeof(double));
847                         offset += size;
848                 }
849         } else {
850                 for(int j = 0; j < numArgs; j++) {
851                         for(int k = 0; k < matList[j]->cols; k++) {
852                                 for(int l = 0; l < totalRows; l++) {            // Gotta be a faster way to do this.
853                                         omxSetMatrixElement(result, l, currentCol, omxMatrixElement(matList[j], l, k));
854                                 }
855                                 currentCol++;
856                         }
857                 }
858         }
859
860 }
861
862 void omxMatrixVertCat(omxMatrix** matList, int numArgs, omxMatrix* result)
863 {
864         int totalRows = 0, totalCols = 0, currentRow=0;
865
866         if(numArgs == 0) return;
867
868         totalCols = matList[0]->cols;                   // Assumed constant.  Assert this below.
869
870         for(int j = 0; j < numArgs; j++) {
871                 if(totalCols != matList[j]->cols) {
872                         char *errstr = (char*) calloc(250, sizeof(char));
873                         sprintf(errstr, "Non-conformable matrices in vertical concatenation (rbind). First argument has %d cols, and argument #%d has %d cols.", totalCols, j + 1, matList[j]->cols);
874                         omxRaiseError(result->currentState, -1, errstr);
875                         free(errstr);
876                         return;
877                 }
878                 totalRows += matList[j]->rows;
879         }
880
881         if(result->rows != totalRows || result->cols != totalCols) {
882                 omxResizeMatrix(result, totalRows, totalCols, FALSE);
883         }
884
885         int allArgumentsRowMajor = !result->colMajor;
886         for(int j = 0; j < numArgs && allArgumentsRowMajor; j++) {
887                 if (matList[j]->colMajor) allArgumentsRowMajor = 0;
888         }
889
890         if (allArgumentsRowMajor) {
891                 int offset = 0;
892                 for(int j = 0; j < numArgs; j++) {      
893                         omxMatrix* current = matList[j];
894                         int size = current->rows * current->cols;       
895                         memcpy(result->data + offset, current->data, size * sizeof(double));
896                         offset += size;
897                 }
898         } else {
899                 for(int j = 0; j < numArgs; j++) {
900                         for(int k = 0; k < matList[j]->rows; k++) {
901                                 for(int l = 0; l < totalCols; l++) {            // Gotta be a faster way to do this.
902                                         omxSetMatrixElement(result, currentRow, l, omxMatrixElement(matList[j], k, l));
903                                 }
904                                 currentRow++;
905                         }
906                 }
907         }
908
909 }
910
911 void omxMatrixDeterminant(omxMatrix** matList, int numArgs, omxMatrix* result)
912 {
913         omxMatrix* inMat = matList[0];
914         omxMatrix* calcMat;                                     // This should be preallocated.
915
916         int rows = inMat->rows;
917         int cols = inMat->cols;
918         double det = 1;
919         int info;
920
921         if(rows != cols) {
922                 char *errstr = (char*) calloc(250, sizeof(char));
923                 sprintf(errstr, "Determinant of non-square matrix cannot be found.\n");
924                 omxRaiseError(result->currentState, -1, errstr);
925                 free(errstr);
926                 return;
927         }
928
929         if(result->rows != 1 || result->cols != 1) {
930                 omxResizeMatrix(result, 1, 1, FALSE);
931         }
932
933         calcMat = omxInitTemporaryMatrix(NULL, rows, cols, TRUE, inMat->currentState);
934         omxCopyMatrix(calcMat, inMat);
935
936         int* ipiv = (int*) calloc(inMat->rows, sizeof(int));
937
938         F77_CALL(dgetrf)(&(calcMat->rows), &(calcMat->cols), calcMat->data, &(calcMat->cols), ipiv, &info);
939
940         if(info != 0) {
941                 char *errstr = (char*) calloc(250, sizeof(char));
942                 sprintf(errstr, "Determinant Calculation: Nonsingular matrix (at row %d) on LUP decomposition.", info);
943                 omxRaiseError(result->currentState, -1, errstr);
944                 free(errstr);
945                 free(ipiv);
946                 omxFreeAllMatrixData(calcMat);
947                 return;
948         }
949
950         if(OMX_DEBUG_ALGEBRA) {
951                 omxPrint(calcMat, "LU Decomp");
952                 mxLog("info is %d.", info);
953         }
954
955         for(int i = 0; i < rows; i++) {
956                 det *= omxMatrixElement(calcMat, i, i);
957                 if(ipiv[i] != (i+1)) det *= -1;
958         }
959
960         if(OMX_DEBUG_ALGEBRA) {
961                 mxLog("det is %f.", det);
962         }
963
964         omxFreeAllMatrixData(calcMat);
965
966         omxSetMatrixElement(result, 0, 0, det);
967
968         free(ipiv);
969 }
970
971 void omxMatrixTrace(omxMatrix** matList, int numArgs, omxMatrix* result)
972 {
973         /* Consistency check: */
974         if(result->rows != numArgs && result->cols != numArgs) {
975                 omxResizeMatrix(result, numArgs, 1, FALSE);
976         }
977
978     for(int i = 0; i < numArgs; i++) {
979         double trace = 0.0;
980         omxMatrix* inMat = matList[i];
981         double* values = inMat->data;
982         int nrow  = inMat->rows;
983         int ncol  = inMat->cols;
984
985         if(nrow != ncol) {
986                 char *errstr = (char*) calloc(250, sizeof(char));
987                 sprintf(errstr, "Non-square matrix in Trace().\n");
988                 omxRaiseError(result->currentState, -1, errstr);
989                 free(errstr);
990             return;
991         }
992
993         /* Note: This algorithm is numerically unstable.  Sorry, dudes. */
994         for(int j = 0; j < nrow; j++)
995            trace += values[j * nrow + j];
996
997         omxSetVectorElement(result, i, trace);
998         }
999 };
1000
1001 void omxMatrixTotalSum(omxMatrix** matList, int numArgs, omxMatrix* result)
1002 {
1003         /* Consistency check: */
1004         if(result->rows != 1 || result->cols != 1) {
1005                 omxResizeMatrix(result, 1, 1, FALSE);
1006         }
1007
1008         double sum = 0.0;
1009
1010         /* Note: This algorithm is numerically unstable.  Sorry, dudes. */
1011         for(int j = 0; j < numArgs; j++) {
1012                 double* data = matList[j]->data;
1013                 int matRf_length = matList[j]->rows * matList[j]->cols;
1014                 for(int k = 0; k < matRf_length; k++) {
1015                         sum += data[k];
1016                 }
1017         }
1018
1019         omxSetMatrixElement(result, 0, 0, sum);
1020 }
1021
1022 void omxMatrixTotalProduct(omxMatrix** matList, int numArgs, omxMatrix* result)
1023 {
1024         /* Consistency check: */
1025         if(result->rows != 1 || result->cols != 1) {
1026                 omxResizeMatrix(result, 1, 1, FALSE);
1027         }
1028
1029         double product = 1.0;
1030
1031         /* Note: This algorithm is numerically unstable.  Sorry, dudes. */
1032         for(int j = 0; j < numArgs; j++) {
1033                 double* data = matList[j]->data;
1034                 int matRf_length = matList[j]->rows * matList[j]->cols;
1035                 for(int k = 0; k < matRf_length; k++) {
1036                         product *= data[k];
1037                 }
1038         }
1039
1040         omxSetMatrixElement(result, 0, 0, product);
1041 }
1042
1043 void omxMatrixArithmeticMean(omxMatrix** matList, int numArgs, omxMatrix* result)
1044 {
1045         /* Consistency check: */
1046         if(result->rows != 1 || result->cols != 1) {
1047                 omxResizeMatrix(result, 1, 1, FALSE);
1048         }
1049
1050         omxMatrix *input = matList[0];
1051         int matLength = input->rows * input->cols;
1052         if (matLength == 0) return;
1053         double mean = omxVectorElement(input, 0);
1054         for(int i = 1; i < matLength; i++) {
1055                 double val = omxVectorElement(input, i);
1056                 mean += (val - mean) / (i + 1); 
1057         }
1058
1059         omxSetMatrixElement(result, 0, 0, mean);
1060 }
1061
1062 void omxMatrixMinimum(omxMatrix** matList, int numArgs, omxMatrix* result)
1063 {
1064         /* Consistency check: */
1065         if(result->rows != 1 || result->cols != 1) {
1066                 omxResizeMatrix(result, 1, 1, FALSE);
1067         }
1068
1069         double min = DBL_MAX; // DBL_MAX is the maximum possible DOUBLE value, usually 10e37.
1070                                                   // We could change this to use NPSOL's INFINITY, but why bother?
1071
1072         for(int j = 0; j < numArgs; j++) {
1073                 double* data = matList[j]->data;
1074                 int matRf_length = matList[j]->rows * matList[j]->cols;
1075                 for(int k = 0; k < matRf_length; k++) {
1076                         if(data[k] < min) min = data[k];
1077                 }
1078         }
1079
1080         omxSetMatrixElement(result, 0, 0, min);
1081 }
1082
1083 void omxMatrixMaximum(omxMatrix** matList, int numArgs, omxMatrix* result)
1084 {
1085         /* Consistency check: */
1086         if(result->rows != 1 || result->cols != 1) {
1087                 omxResizeMatrix(result, 1, 1, FALSE);
1088         }
1089
1090         double max = -DBL_MAX;
1091
1092         for(int j = 0; j < numArgs; j++) {
1093                 double* data = matList[j]->data;
1094                 int matRf_length = matList[j]->rows * matList[j]->cols;
1095                 for(int k = 0; k < matRf_length; k++) {
1096                         if(data[k] > max) max = data[k];
1097                 }
1098         }
1099
1100         omxSetMatrixElement(result, 0, 0, max);
1101 }
1102
1103 void omxMatrixAbsolute(omxMatrix** matList, int numArgs, omxMatrix* result)
1104 {
1105         omxMatrix* inMat = matList[0];
1106
1107         int max = inMat->cols * inMat->rows;
1108
1109         omxCopyMatrix(result, inMat);
1110
1111         double* data = result->data;
1112         for(int j = 0; j < max; j++) {
1113                 data[j] = fabs(data[j]);
1114         }
1115
1116 }
1117
1118 void omxMatrixDiagonal(omxMatrix** matList, int numArgs, omxMatrix* result)
1119 {
1120         omxMatrix* inMat = matList[0];
1121         int diags = inMat->cols;
1122         if(inMat->cols > inMat->rows) {
1123                 diags = inMat->rows;
1124         }
1125
1126         if (result->cols != 1 || result->rows != diags) {
1127                 omxResizeMatrix(result, diags, 1, FALSE);
1128         }
1129
1130         for(int j = 0; j < diags; j++) {
1131                 omxSetMatrixElement(result, j, 0, omxMatrixElement(inMat, j, j));
1132         }
1133
1134 }
1135
1136 void omxMatrixFromDiagonal(omxMatrix** matList, int numArgs, omxMatrix* result)
1137 {
1138         omxMatrix* inMat = matList[0];
1139         int diags = inMat->cols;
1140
1141         if(inMat->cols < inMat->rows) {
1142                 diags = inMat->rows;
1143         }
1144
1145         if(inMat->cols != 1 && inMat->rows != 1) {
1146                 char *errstr = (char*) calloc(250, sizeof(char));
1147                 sprintf(errstr, "To generate a matrix from a diagonal that is not 1xN or Nx1.");
1148                 omxRaiseError(result->currentState, -1, errstr);
1149                 free(errstr);
1150                 return;
1151         }
1152
1153         if (result->cols != diags || result->rows != diags) {
1154                         omxResizeMatrix(result, diags, diags, FALSE);
1155         }
1156
1157         for(int j = 0; j < diags; j++) {
1158                 for(int k = 0; k < diags; k++) {
1159                         if(j == k) {
1160                                 omxSetMatrixElement(result, j, k, omxVectorElement(inMat, j));
1161                         } else {
1162                                 omxSetMatrixElement(result, j, k, 0);
1163                         }
1164                 }
1165         }
1166 }
1167
1168 void omxElementCosine(omxMatrix** matList, int numArgs, omxMatrix* result)
1169 {
1170         omxMatrix* inMat = matList[0];
1171
1172         int max = inMat->cols * inMat->rows;
1173
1174         omxCopyMatrix(result, inMat);
1175
1176         double* data = result->data;
1177         for(int j = 0; j < max; j++) {
1178                 data[j] = cos(data[j]);
1179         }
1180
1181 }
1182
1183 void omxElementCosh(omxMatrix** matList, int numArgs, omxMatrix* result)
1184 {
1185         omxMatrix* inMat = matList[0];
1186
1187         int max = inMat->cols * inMat->rows;
1188
1189         omxCopyMatrix(result, inMat);
1190
1191         double* data = result->data;
1192         for(int j = 0; j < max; j++) {
1193                 data[j] = cosh(data[j]);
1194         }
1195
1196 }
1197
1198 void omxElementSine(omxMatrix** matList, int numArgs, omxMatrix* result)
1199 {
1200         omxMatrix* inMat = matList[0];
1201
1202         int max = inMat->cols * inMat->rows;
1203
1204         omxCopyMatrix(result, inMat);
1205
1206         double* data = result->data;
1207         for(int j = 0; j < max; j++) {
1208                 data[j] = sin(data[j]);
1209         }
1210
1211 }
1212
1213 void omxElementSinh(omxMatrix** matList, int numArgs, omxMatrix* result)
1214 {
1215         omxMatrix* inMat = matList[0];
1216
1217         int max = inMat->cols * inMat->rows;
1218
1219         omxCopyMatrix(result, inMat);
1220
1221         double* data = result->data;
1222         for(int j = 0; j < max; j++) {
1223                 data[j] = sinh(data[j]);
1224         }
1225
1226 }
1227
1228 void omxElementTangent(omxMatrix** matList, int numArgs, omxMatrix* result)
1229 {
1230         omxMatrix* inMat = matList[0];
1231
1232         int max = inMat->cols * inMat->rows;
1233
1234         omxCopyMatrix(result, inMat);
1235
1236         double* data = result->data;
1237         for(int j = 0; j < max; j++) {
1238                 data[j] = tan(data[j]);
1239         }
1240
1241 }
1242
1243 void omxElementTanh(omxMatrix** matList, int numArgs, omxMatrix* result)
1244 {
1245         omxMatrix* inMat = matList[0];
1246
1247         int max = inMat->cols * inMat->rows;
1248
1249         omxCopyMatrix(result, inMat);
1250
1251         double* data = result->data;
1252         for(int j = 0; j < max; j++) {
1253                 data[j] = tanh(data[j]);
1254         }
1255
1256 }
1257
1258 void omxElementExponent(omxMatrix** matList, int numArgs, omxMatrix* result)
1259 {
1260         omxMatrix* inMat = matList[0];
1261
1262         int max = inMat->cols * inMat->rows;
1263
1264         omxCopyMatrix(result, inMat);
1265
1266         double* data = result->data;
1267         for(int j = 0; j < max; j++) {
1268                 data[j] = exp(data[j]);
1269         }
1270
1271 }
1272
1273 void omxElementNaturalLog(omxMatrix** matList, int numArgs, omxMatrix* result)
1274 {
1275         omxMatrix* inMat = matList[0];
1276
1277         int max = inMat->cols * inMat->rows;
1278
1279         omxCopyMatrix(result, inMat);
1280
1281         double* data = result->data;
1282         for(int j = 0; j < max; j++) {
1283                 data[j] = log(data[j]);
1284         }
1285
1286 }
1287
1288 void omxElementSquareRoot(omxMatrix** matList, int numArgs, omxMatrix* result)
1289 {
1290         omxMatrix *inMat = matList[0];
1291
1292         int max = inMat->cols * inMat->rows;
1293
1294         omxCopyMatrix(result, inMat);
1295
1296         double* data = result->data;
1297         for(int j = 0; j < max; j++) {
1298                 data[j] = sqrt(data[j]);
1299         }
1300 }
1301
1302 void omxMatrixVech(omxMatrix** matList, int numArgs, omxMatrix* result) {
1303         omxMatrix *inMat = matList[0];
1304
1305         int size;
1306         if (inMat->rows > inMat->cols) {
1307                 size = inMat->cols * (2 * inMat->rows - inMat->cols + 1) / 2;
1308         } else {
1309                 size = inMat->rows * (inMat->rows + 1) / 2;
1310         }
1311
1312         /* Consistency check: */
1313         if(result->rows != size || result->cols != 1) {
1314                 omxResizeMatrix(result, size, 1, FALSE);
1315         }
1316
1317         int counter = 0;
1318         for(int i = 0; i < inMat->cols; i++) {
1319                 for(int j = i; j < inMat->rows; j++) {
1320                         omxSetMatrixElement(result, counter, 0, omxMatrixElement(inMat, j, i));
1321                         counter++;
1322                 }
1323         }
1324
1325         if(counter != size) {
1326                 char *errstr = (char*) calloc(250, sizeof(char));
1327                 sprintf(errstr, "Internal Rf_error in vech().\n");
1328                 omxRaiseError(result->currentState, -1, errstr);
1329                 free(errstr);
1330         }
1331
1332 }
1333
1334 void omxMatrixVechs(omxMatrix** matList, int numArgs, omxMatrix* result) {
1335         omxMatrix *inMat = matList[0];
1336
1337         int size;
1338         if (inMat->rows > inMat->cols) {
1339                 size = inMat->cols * (2 * inMat->rows - inMat->cols + 1) / 2 - inMat->cols;
1340         } else {
1341                 size = inMat->rows * (inMat->rows + 1) / 2 - inMat->rows;
1342         }
1343
1344         /* Consistency check: */
1345         if(result->rows != size || result->cols != 1) {
1346                 omxResizeMatrix(result, size, 1, FALSE);
1347         }
1348
1349         int counter = 0;
1350         for(int i = 0; i < inMat->cols; i++) {
1351                 for(int j = i + 1; j < inMat->rows; j++) {
1352                         omxSetMatrixElement(result, counter, 0, omxMatrixElement(inMat, j, i));
1353                         counter++;
1354                 }
1355         }
1356
1357         if(counter != size) {
1358                 char *errstr = (char*) calloc(250, sizeof(char));
1359                 sprintf(errstr, "Internal Rf_error in vechs().\n");
1360                 omxRaiseError(result->currentState, -1, errstr);
1361                 free(errstr);
1362         }
1363
1364 }
1365
1366 void omxRowVectorize(omxMatrix** matList, int numArgs, omxMatrix* result)
1367 {
1368         omxMatrix *inMat = matList[0];
1369
1370         int size = (inMat->rows * inMat->cols);
1371
1372         /* Consistency Check */
1373         if(result->rows != size || result->cols != 1)
1374                 omxResizeMatrix(result, size, 1, FALSE);
1375
1376         if(!inMat->colMajor) {          // Special case: we can just memcpy.
1377                 memcpy(result->data, inMat->data, size*sizeof(double));
1378         } else {
1379                 int next = 0;
1380                 for(int i = 0; i < inMat->rows; i++) {
1381                         for(int j = 0; j < inMat->cols; j++) {
1382                                 omxSetMatrixElement(result, next++, 0, omxMatrixElement(inMat, i, j));
1383                         }
1384                 }
1385         }
1386 }
1387
1388 void omxColVectorize(omxMatrix** matList, int numArgs, omxMatrix* result)
1389 {
1390         omxMatrix *inMat = matList[0];
1391
1392         int size = (inMat->rows * inMat->cols);
1393
1394         /* Consistency Check */
1395         if(result->rows != size || result->cols != 1)
1396                 omxResizeMatrix(result, size, 1, FALSE);
1397         if(inMat->colMajor) {           // Special case: we can just memcpy.
1398                 memcpy(result->data, inMat->data, size * sizeof(double));
1399         } else {
1400                 int next = 0;
1401                 for(int i = 0; i < inMat->cols; i++) {
1402                         for(int j = 0; j < inMat->rows; j++) {
1403                                 omxSetMatrixElement(result, next++, 0, omxMatrixElement(inMat, j, i));
1404                         }
1405                 }
1406         }
1407 }
1408
1409
1410 void omxSequenceGenerator(omxMatrix** matList, int numArgs, omxMatrix* result) {
1411
1412         double start = omxVectorElement(matList[0], 0);
1413         double stop = omxVectorElement(matList[1], 0);
1414
1415         if (!R_finite(start)) {
1416                 char *errstr = (char*) calloc(250, sizeof(char));
1417                 sprintf(errstr, "Non-finite start value in ':' operator.\n");
1418                 omxRaiseError(result->currentState, -1, errstr);
1419                 free(errstr);
1420                 return;
1421         }
1422
1423         if (!R_finite(stop)) {
1424                 char *errstr = (char*) calloc(250, sizeof(char));
1425                 sprintf(errstr, "Non-finite stop value in ':' operator.\n");
1426                 omxRaiseError(result->currentState, -1, errstr);
1427                 free(errstr);
1428                 return;
1429         }
1430
1431         double difference = stop - start;
1432         if (difference < 0) difference = - difference;
1433
1434         int size = ((int) difference) + 1;
1435
1436         /* Consistency check: */
1437         if(result->rows != size || result->cols != 1) {
1438                 omxResizeMatrix(result, size, 1, FALSE);
1439         }
1440
1441         /* Sanity-checking.  This loop can be eliminated */
1442         for(int i = 0; i < size; i++) {
1443                 omxSetVectorElement(result, i, 0);
1444         }
1445
1446         int count = 0;
1447         if ((stop - start) >= 0) {
1448                 while (start <= stop) {
1449                         omxSetVectorElement(result, count, start);
1450                         start = start + 1.0;
1451                         count++;
1452                 }
1453         } else {
1454                 while (start >= stop) {
1455                         omxSetVectorElement(result, count, start);
1456                         start = start - 1.0;
1457                         count++;
1458                 }
1459         }
1460 }
1461
1462 void omxMultivariateNormalIntegration(omxMatrix** matList, int numArgs, omxMatrix* result) {
1463
1464         omxMatrix* cov = matList[0];
1465         omxMatrix* means = matList[1];
1466         omxMatrix* lBoundMat = matList[2];
1467         omxMatrix* uBoundMat = matList[3];
1468
1469         /* Conformance checks: */
1470         if (result->rows != 1 || result->cols != 1) omxResizeMatrix(result, 1, 1, FALSE);
1471
1472         if (cov->rows != cov->cols) {
1473                 char *errstr = (char*) calloc(250, sizeof(char));
1474                 sprintf(errstr, "covariance is not a square matrix");
1475                 omxRaiseError(result->currentState, -1, errstr);
1476                 free(errstr);
1477                 return;
1478         }
1479
1480         if (means->rows > 1 && means->cols > 1) {
1481                 char *errstr = (char*) calloc(250, sizeof(char));
1482                 sprintf(errstr, "means is neither row nor column vector");
1483                 omxRaiseError(result->currentState, -1, errstr);
1484                 free(errstr);
1485                 return;
1486         }
1487
1488         if (lBoundMat->rows > 1 && lBoundMat->cols > 1) {
1489                 char *errstr = (char*) calloc(250, sizeof(char));
1490                 sprintf(errstr, "lbound is neither row nor column vector");
1491                 omxRaiseError(result->currentState, -1, errstr);
1492                 free(errstr);
1493                 return;
1494         }
1495
1496         if (uBoundMat->rows > 1 && uBoundMat->cols > 1) {
1497                 char *errstr = (char*) calloc(250, sizeof(char));
1498                 sprintf(errstr, "ubound is neither row nor column vector");
1499                 omxRaiseError(result->currentState, -1, errstr);
1500                 free(errstr);
1501                 return;
1502         }
1503
1504         int nElements = (cov->cols > 1) ? cov->cols : cov->rows;
1505         double *lBounds, *uBounds;
1506         double *weights;
1507         double *corList;
1508         lBounds = (double*) malloc(nElements * sizeof(double));
1509         uBounds = (double*) malloc(nElements * sizeof(double));
1510         weights = (double*) malloc(nElements * sizeof(double));
1511         corList = (double*) malloc((nElements * (nElements + 1) / 2) * sizeof(double));
1512
1513         omxStandardizeCovMatrix(cov, corList, weights);
1514
1515         // SADMVN calls Alan Genz's sadmvn.f--see appropriate file for licensing info.
1516         // TODO: Check with Genz: should we be using sadmvn or sadmvn?
1517         // Parameters are:
1518         //      N               int                     # of vars
1519         //      Lower   double*         Array of lower bounds
1520         //      Upper   double*         Array of upper bounds
1521         //      Infin   int*            Array of flags: <0 = (-Inf, Inf) 0 = (-Inf, upper] 1 = [lower, Inf), 2 = [lower, upper]
1522         //      Correl  double*         Array of correlation coeffs: in row-major lower triangular order
1523         //      MaxPts  int                     Maximum # of function values (use 1000*N or 1000*N*N)
1524         //      Abseps  double          Absolute Rf_error tolerance.  Yick.
1525         //      Releps  double          Relative Rf_error tolerance.  Use EPSILON.
1526         //      Error   &double         On return: absolute real Rf_error, 99% confidence
1527         //      Value   &double         On return: evaluated value
1528         //      Inform  &int            On return: 0 = OK; 1 = Rerun, increase MaxPts; 2 = Bad input
1529         // TODO: Separate block diagonal covariance matrices into pieces for integration separately
1530         double Error;
1531         double absEps = 1e-3;
1532         double relEps = 0;
1533         int MaxPts = OMX_DEFAULT_MAX_PTS(cov->rows);
1534         double likelihood;
1535         int inform;
1536         int numVars = cov->rows;
1537         int Infin[cov->rows];
1538         int fortranThreadId = omx_absolute_thread_num() + 1;
1539
1540         for(int i = 0; i < nElements; i++) {
1541                 lBounds[i] = (omxVectorElement(lBoundMat, i) - omxVectorElement(means, i))/weights[i];
1542                 uBounds[i] = (omxVectorElement(uBoundMat, i) - omxVectorElement(means, i))/weights[i];
1543                 Infin[i] = 2; // Default to both thresholds
1544                 if(uBounds[i] <= lBounds[i]) {
1545                         char *errstr = (char*) calloc(250, sizeof(char));
1546                         sprintf(errstr, "Thresholds are not strictly increasing: %3.3f >= %3.3f.", lBounds[i], uBounds[i]);
1547                         omxRaiseError(result->currentState, -1, errstr);
1548                         free(errstr);
1549                         free(corList);
1550                         free(weights);
1551                         free(uBounds);
1552                         free(lBounds);
1553                         return;
1554                 }
1555                 if(!R_finite(lBounds[i]) ) {
1556                         Infin[i] -= 2;  // NA or INF or -INF means no lower threshold.
1557                 } else {
1558
1559                 }
1560                 if(!R_finite(uBounds[i]) ) {
1561                         Infin[i] -= 1; // NA or INF or -INF means no upper threshold.
1562                 }
1563
1564         }
1565
1566
1567         F77_CALL(sadmvn)(&numVars, &(lBounds[0]), &(*uBounds), Infin, corList, 
1568                 &MaxPts, &absEps, &relEps, &Error, &likelihood, &inform, &fortranThreadId);
1569
1570         if(OMX_DEBUG_ALGEBRA) { mxLog("Output of sadmvn is %f, %f, %d.", Error, likelihood, inform); }
1571
1572         if(inform == 2) {
1573                 char *errstr = (char*) calloc(250, sizeof(char));
1574                 sprintf(errstr, "Improper input to sadmvn.");
1575                 omxRaiseError(result->currentState, -1, errstr);
1576                 free(errstr);
1577                 free(corList);
1578                 free(weights);
1579                 free(uBounds);
1580                 free(lBounds);
1581                 return;
1582         }
1583
1584         free(corList);
1585         free(weights);
1586         free(uBounds);
1587         free(lBounds);
1588
1589         omxSetMatrixElement(result, 0, 0, likelihood);
1590
1591 }
1592
1593 void omxAllIntegrationNorms(omxMatrix** matList, int numArgs, omxMatrix* result)
1594 {
1595         omxMatrix* cov = matList[0];
1596         omxMatrix* means = matList[1];
1597         int nCols = cov->cols;
1598         int i,j,k;
1599
1600         int totalLevels = 1;
1601         omxMatrix **thresholdMats = (omxMatrix **) malloc(nCols * sizeof(omxMatrix*));
1602         int *numThresholds = (int*) malloc(nCols * sizeof(int));
1603         int *matNums = (int*) malloc(nCols * sizeof(int));
1604         int *thresholdCols = (int*) malloc(nCols * sizeof(int));
1605         int *currentThresholds = (int*) malloc(nCols * sizeof(int));
1606
1607         int currentMat = 0;
1608
1609         for(i = currentMat; i < nCols;) {                                                       // Map out the structure of levels.
1610         if(OMX_DEBUG_ALGEBRA) {
1611                 mxLog("All-part multivariate normal integration: Examining threshold column %d.", i);
1612         }
1613                 thresholdMats[currentMat] = matList[currentMat+2];              // Get the thresholds for this covariance column
1614
1615                 for(j = 0; j < thresholdMats[currentMat]->cols; j++) {  // We walk along the columns of this threshold matrix
1616                         double ubound, lbound = omxMatrixElement(thresholdMats[currentMat], 0, j);
1617                         if(ISNA(lbound)) {
1618                                 char *errstr = (char*) calloc(250, sizeof(char));
1619                                 sprintf(errstr, "Invalid lowest threshold for dimension %d of Allint.", j);
1620                                 omxRaiseError(result->currentState, -1, errstr);
1621                                 free(errstr);
1622                                 return;
1623                         }
1624
1625                         thresholdCols[i] = j;
1626
1627                         for(k = 1; k < thresholdMats[currentMat]->rows; k++) {
1628                                 ubound = omxMatrixElement(thresholdMats[currentMat], k, j);
1629                                 if(ISNA(ubound)) {
1630                                         numThresholds[i] = k-1;
1631                                         totalLevels *= numThresholds[i];
1632                                         break;
1633                                 }
1634
1635                                 if(!(ubound > lbound)) {
1636                                         char *errstr = (char*) calloc(250, sizeof(char));
1637                                         sprintf(errstr, "Thresholds (%f and %f) are not strictly increasing for dimension %d of Allint.", lbound, ubound, j+1);
1638                                         omxRaiseError(result->currentState, -1, errstr);
1639                                         free(errstr);
1640                                         return;
1641                                 }
1642
1643                                 if(!R_finite(ubound)) {                                 // Infinite bounds must be last.
1644                                         numThresholds[i] = k;
1645                                         totalLevels *= numThresholds[i];
1646                                         break;
1647                                 }
1648
1649                                 if(k == (thresholdMats[currentMat]->rows -1)) { // In case the highest threshold isn't Infinity
1650                                         numThresholds[i] = k;
1651                                         totalLevels *= numThresholds[i];
1652                                 }
1653                         }
1654                         currentThresholds[i] = 1;
1655                         matNums[i] = currentMat;
1656                         if(++i >= nCols) {                                                      // We have all we need
1657                                 break;
1658                         }
1659                 }
1660                 currentMat++;
1661         }
1662
1663         /* Conformance checks: */
1664         if(result->rows != totalLevels || result->cols != 1) omxResizeMatrix(result, totalLevels, 1, FALSE);
1665
1666         double *weights = (double*) malloc(nCols * sizeof(double));
1667         double *corList = (double*) malloc((nCols * (nCols + 1) / 2) * sizeof(double));
1668
1669         omxStandardizeCovMatrix(cov, &(*corList), &(*weights));
1670
1671         // SADMVN calls Alan Genz's sadmvn.f--see appropriate file for licensing info.
1672         // TODO: Check with Genz: should we be using sadmvn or sadmvn?
1673         // Parameters are:
1674         //      N               int                     # of vars
1675         //      Lower   double*         Array of lower bounds
1676         //      Upper   double*         Array of upper bounds
1677         //      Infin   int*            Array of flags: <0 = (-Inf, Inf) 0 = (-Inf, upper] 1 = [lower, Inf), 2 = [lower, upper]
1678         //      Correl  double*         Array of correlation coeffs: in row-major lower triangular order
1679         //      MaxPts  int                     Maximum # of function values (use 1000*N or 1000*N*N)
1680         //      Abseps  double          Absolute Rf_error tolerance.  Yick.
1681         //      Releps  double          Relative Rf_error tolerance.  Use EPSILON.
1682         //      Error   &double         On return: absolute real Rf_error, 99% confidence
1683         //      Value   &double         On return: evaluated value
1684         //      Inform  &int            On return: 0 = OK; 1 = Rerun, increase MaxPts; 2 = Bad input
1685         // TODO: Separate block diagonal covariance matrices into pieces for integration separately
1686         double Error;
1687         double absEps = 1e-3;
1688         double relEps = 0;
1689         int MaxPts = OMX_DEFAULT_MAX_PTS(cov->rows);
1690         double likelihood;
1691         int inform;
1692         int numVars = nCols;
1693         int* Infin = (int*) malloc(nCols * sizeof(int));
1694         double* lBounds = (double*) malloc(nCols * sizeof(double));
1695         double* uBounds = (double*) malloc(nCols * sizeof(double));
1696         int fortranThreadId = omx_absolute_thread_num() + 1;
1697
1698         /* Set up first row */
1699         for(j = (nCols-1); j >= 0; j--) {                                       // For each threshold set, starting from the fastest
1700
1701                 Infin[j] = 2;                                                                   // Default to using both thresholds
1702                 lBounds[j] = (omxMatrixElement(thresholdMats[matNums[j]], currentThresholds[j]-1, thresholdCols[j]) - omxVectorElement(means, j))/weights[j];
1703                 if(!R_finite(lBounds[j])) {                                     // Inifinite lower bounds = -Inf to ?
1704                                 Infin[j] -= 2;
1705                 }
1706
1707                 uBounds[j] = (omxMatrixElement(thresholdMats[matNums[j]], currentThresholds[j], thresholdCols[j]) - omxVectorElement(means, j))/weights[j];
1708
1709                 if(!R_finite(uBounds[j])) {                                     // Inifinite lower bounds = -Inf to ?
1710                                 Infin[j] -= 1;
1711                 }
1712
1713                 if(Infin[j] < 0) { Infin[j] = 3; }                      // Both bounds infinite.
1714         }
1715
1716         F77_CALL(sadmvn)(&numVars, &(lBounds[0]), &(*uBounds), Infin, corList, 
1717                 &MaxPts, &absEps, &relEps, &Error, &likelihood, &inform, &fortranThreadId);
1718
1719         if(OMX_DEBUG_ALGEBRA) { mxLog("Output of sadmvn is %f, %f, %d.", Error, likelihood, inform); }
1720
1721         if(inform == 2) {
1722                 char *errstr = (char*) calloc(250, sizeof(char));
1723                 sprintf(errstr, "Improper input to sadmvn.");
1724                 omxRaiseError(result->currentState, -1, errstr);
1725                 free(errstr);
1726                 goto AllIntCleanup;
1727         }
1728
1729         omxSetMatrixElement(result, 0, 0, likelihood);
1730
1731
1732         /* And repeat with increments for all other rows. */
1733         for(i = 1; i < totalLevels; i++) {
1734                 for(j = (nCols-1); j >= 0; j--) {                                                       // For each threshold set, starting from the fastest
1735                         currentThresholds[j]++;                                                                 // Move to the next threshold set.
1736                         if(currentThresholds[j] > numThresholds[j]) {                   // Hit the end; cycle to the next.
1737                                 currentThresholds[j] = 1;
1738                         }
1739
1740                         /* Update only the rows that need it. */
1741                         Infin[j] = 2; // Default to both thresholds
1742                         lBounds[j] = (omxMatrixElement(thresholdMats[matNums[j]], currentThresholds[j]-1, thresholdCols[j]) - omxVectorElement(means, j))/weights[j];
1743                         if(!R_finite(lBounds[j])) {                                                             // Inifinite lower bounds = -Inf to ?
1744                                 Infin[j] -= 2;
1745                         }
1746                         uBounds[j] = (omxMatrixElement(thresholdMats[matNums[j]], currentThresholds[j], thresholdCols[j]) - omxVectorElement(means, j))/weights[j];
1747
1748                         if(!R_finite(uBounds[j])) {                                                     // Inifinite lower bounds = -Inf to ?
1749                                 Infin[j] -= 1;
1750                         }
1751
1752                         if(Infin[j] < 0) { Infin[j] = 3; }                                              // Both bounds infinite.
1753
1754                         if(currentThresholds[j] != 1) {                                                 // If we just cycled, we need to see the next set.
1755                                 break;
1756                         }
1757
1758                 }
1759
1760                 F77_CALL(sadmvn)(&numVars, &(lBounds[0]), &(*uBounds), Infin, corList,
1761                         &MaxPts, &absEps, &relEps, &Error, &likelihood, &inform, &fortranThreadId);
1762
1763                 if(OMX_DEBUG_ALGEBRA) { mxLog("Output of sadmvn is %f, %f, %d.", Error, likelihood, inform); }
1764
1765                 if(inform == 2) {
1766                         char *errstr = (char*) calloc(250, sizeof(char));
1767                         sprintf(errstr, "Improper input to sadmvn.");
1768                         omxRaiseError(result->currentState, -1, errstr);
1769                         free(errstr);
1770                         goto AllIntCleanup;
1771                 }
1772
1773                 omxSetMatrixElement(result, i, 0, likelihood);
1774         }
1775
1776 AllIntCleanup:
1777         free(Infin);
1778         free(lBounds);
1779         free(uBounds);
1780         free(weights);
1781         free(corList);
1782         free(thresholdMats);
1783         free(numThresholds);
1784         free(matNums);
1785         free(thresholdCols);
1786         free(currentThresholds);
1787 }
1788
1789 int omxCompareDoubleHelper(const void* one, const void* two, void *ign) {
1790         double diff = *(double*) two - *(double*) one;
1791         if(diff > EPSILON) {
1792                 return 1;
1793         } else if(diff < -EPSILON) {
1794                 return -1;
1795         } else return 0;
1796 }
1797
1798
1799 int omxComparePointerContentsHelper(const void* one, const void* two, void *ign) {
1800         double diff = (*(*(double**) two)) - (*(*(double**) one));
1801         if(diff > EPSILON) {
1802                 return 1;
1803         } else if(diff < -EPSILON) {
1804                 return -1;
1805         } else return 0;
1806 }
1807
1808 void omxSortHelper(double* sortOrder, omxMatrix* original, omxMatrix* result) {
1809         /* Sorts the columns of a matrix or the rows of a column vector
1810                                         in decreasing order of the elements of sortOrder. */
1811
1812         if(OMX_DEBUG) {mxLog("SortHelper:Original is (%d x %d), result is (%d x %d).", original->rows, original->cols, result->rows, result->cols);}
1813
1814         if(!result->colMajor || !original->colMajor
1815                 || result->cols != original->cols || result->rows != original->rows) {
1816                 char *errstr = (char*) calloc(250, sizeof(char));
1817                 sprintf(errstr, "Incorrect input to omxRowSortHelper: %d %d %d %d", result->cols, original->cols, result->rows, original->rows);
1818                 omxRaiseError(result->currentState, -1, errstr);
1819                 free(errstr);
1820                 return;
1821         }
1822
1823         double* sortArray[original->rows];
1824         int numElements = original->cols;
1825         int numRows = original->rows;
1826
1827         if(numElements == 1)  numElements = numRows;            // Special case for column vectors
1828
1829         for(int i = 0; i < numElements; i++) {
1830                 sortArray[i] = sortOrder + i;
1831         }
1832
1833         freebsd_mergesort(sortArray, numElements, sizeof(double*), omxComparePointerContentsHelper, NULL);
1834
1835         if(OMX_DEBUG) {mxLog("Original is (%d x %d), result is (%d x %d).", original->rows, original->cols, result->rows, result->cols);}
1836
1837
1838         for(int i = 0; i < numElements; i++) {
1839                 if(original->cols == 1) {
1840                         omxSetMatrixElement(result, i, 0, omxMatrixElement(original, (sortArray[i] - sortOrder), 0));
1841                 } else {
1842                         memcpy(omxLocationOfMatrixElement(result, 0, i), omxLocationOfMatrixElement(original, 0, sortArray[i]-sortOrder), numRows * sizeof(double));
1843                 }
1844         }
1845
1846         return;
1847 }
1848
1849 void omxRealEigenvalues(omxMatrix** matList, int numArgs, omxMatrix* result)
1850 {
1851         omxMatrix* A = omxInitMatrix(NULL, 0, 0, TRUE, result->currentState);
1852         omxMatrix* B = omxInitMatrix(NULL, 0, 0, TRUE, result->currentState);
1853         omxCopyMatrix(B, matList[0]);
1854         omxResizeMatrix(A, B->rows, 1, FALSE);
1855
1856         /* Conformability Check! */
1857         if(B->cols != B->rows) {
1858                 char *errstr = (char*) calloc(250, sizeof(char));
1859                 sprintf(errstr, "Non-square matrix in eigenvalue decomposition.\n");
1860                 omxRaiseError(B->currentState, -1, errstr);
1861                 free(errstr);
1862                 omxFreeMatrixData(A);
1863                 omxFreeMatrixData(B);
1864                 return;
1865         }
1866
1867         if(result->rows != B->rows || result->cols != 1)
1868                 omxResizeMatrix(result, B->rows, 1, FALSE);
1869
1870         char N = 'N';                                           // Indicators for BLAS
1871         // char V = 'V';                                                // Indicators for BLAS
1872
1873         int One = 1;
1874         int lwork = 10*B->rows;
1875
1876         int info;
1877
1878         double* work = (double*) malloc(lwork * sizeof(double));
1879         double* WI = (double*) malloc(B->cols * sizeof(double));
1880
1881         F77_CALL(dgeev)(&N, &N, &(B->rows), B->data, &(B->leading), A->data, WI, NULL, &One, NULL, &One, work, &lwork, &info);
1882         if(info != 0) {
1883                 char *errstr = (char*) calloc(250, sizeof(char));
1884                 sprintf(errstr, "DGEEV returned %d in (real) eigenvalue decomposition:", info);
1885                 if(info > 0)
1886                         sprintf(errstr, "%s argument %d had an illegal value.  Post this to the OpenMx wiki.\n", errstr, info);
1887                 else
1888                         sprintf(errstr, "%s Unable to decompose matrix: Not of full rank.\n", errstr);
1889                 omxRaiseError(result->currentState, -1, errstr);
1890                 free(errstr);
1891                 goto RealEigenValCleanup;
1892         }
1893
1894         result->colMajor = TRUE;
1895
1896         // Calculate Eigenvalue modulus.
1897         for(int i = 0; i < A->rows; i++) {
1898                 double value = omxMatrixElement(A, i, 0);
1899                 if(WI[i] != 0) {                                // FIXME: Might need to be abs(WI[i] > EPSILON)
1900                         value = sqrt(WI[i]*WI[i] + value*value);                                // Sort by eigenvalue modulus
1901                         WI[i] = value;
1902                         WI[++i] = value;                                                                                // Conjugate pair.
1903                 } else {
1904                         WI[i] = fabs(value);                                                                    // Modulus of a real is its absolute value
1905                 }
1906         }
1907
1908         omxSortHelper(WI, A, result);
1909
1910 RealEigenValCleanup:
1911         omxFreeMatrixData(A);                           // FIXME: State-keeping for algebras would save significant time in memory allocation/deallocation
1912         omxFreeMatrixData(B);
1913         omxMatrixLeadingLagging(result);
1914
1915         free(work);
1916         free(WI);
1917 }
1918
1919 void omxRealEigenvectors(omxMatrix** matList, int numArgs, omxMatrix* result)
1920 {
1921         omxMatrix* A = omxInitMatrix(NULL, 0, 0, TRUE, result->currentState);
1922         omxCopyMatrix(result, matList[0]);
1923         omxResizeMatrix(A, result->rows, result->cols, FALSE);
1924
1925
1926         if(A == NULL) {
1927                 char *errstr = (char*) calloc(250, sizeof(char));
1928                 sprintf(errstr, "Null matrix pointer detected.\n");
1929                 omxRaiseError(result->currentState, -1, errstr);
1930                 free(errstr);
1931                 return;
1932         }
1933
1934         /* Conformability Check! */
1935         if(A->cols != A->rows) {
1936                 char *errstr = (char*) calloc(250, sizeof(char));
1937                 sprintf(errstr, "Non-square matrix in (real) eigenvalue decomposition.\n");
1938                 omxRaiseError(result->currentState, -1, errstr);
1939                 free(errstr);
1940                 omxFreeMatrixData(A);
1941                 return;
1942         }
1943
1944         char N = 'N';                                           // Indicators for BLAS
1945         char V = 'V';                                           // Indicators for BLAS
1946
1947         int One = 1;
1948         int lwork = 10*A->rows;
1949
1950         int info;
1951
1952         double *WR = (double*) malloc(A->cols * sizeof(double));
1953         double *WI = (double*) malloc(A->cols * sizeof(double));
1954         double *work = (double*) malloc(lwork * sizeof(double));
1955
1956         F77_CALL(dgeev)(&N, &V, &(result->rows), result->data, &(result->leading), WR, WI, NULL, &One, A->data, &(A->leading), work, &lwork, &info);
1957         if(info != 0) {
1958                 char *errstr = (char*) calloc(250, sizeof(char));
1959                 sprintf(errstr, "DGEEV returned %d in eigenvalue decomposition:", info);
1960                 if(info > 0)
1961                         sprintf(errstr, "%s argument %d had an illegal value.  Post this to the OpenMx wiki.\n", errstr, info);
1962                 else
1963                         sprintf(errstr, "%s Unable to decompose matrix: Not of full rank.\n", errstr);
1964                 omxRaiseError(result->currentState, -1, errstr);
1965                 free(errstr);
1966                 goto RealEigenVecCleanup;
1967         }
1968
1969         // Filter real and imaginary eigenvectors.  Real ones have no WI.
1970         for(int i = 0; i < A->cols; i++) {
1971                 if(fabs(WI[i]) > EPSILON) {                                                                     // If this is part of a conjugate pair
1972                         memcpy(omxLocationOfMatrixElement(A, 0, i+1), omxLocationOfMatrixElement(A, 0, i), A->rows * sizeof(double));
1973                                 // ^^ This is column-major, so we can clobber columns over one another.
1974                         WR[i] = sqrt(WR[i] *WR[i] + WI[i]*WI[i]);                               // Sort by eigenvalue modulus
1975                         WR[i+1] = WR[i];                                                                                // Identical--conjugate pair
1976                         i++;    // Skip the next one; we know it's the conjugate pair.
1977                 } else {
1978                         WR[i] = fabs(WR[i]);                                                                    // Modulus of a real is its absolute value
1979                 }
1980         }
1981
1982         result->colMajor = TRUE;
1983
1984         // Sort results
1985         omxSortHelper(WR, A, result);
1986
1987 RealEigenVecCleanup:
1988         omxFreeMatrixData(A);           // FIXME: State-keeping for algebras would save significant time in memory allocation/deallocation
1989         omxMatrixLeadingLagging(result);
1990
1991         free(WR);
1992         free(WI);
1993         free(work);     
1994 }
1995
1996 void omxImaginaryEigenvalues(omxMatrix** matList, int numArgs, omxMatrix* result)
1997 {
1998         omxMatrix* A = omxInitMatrix(NULL, 0, 0, TRUE, result->currentState);
1999         omxMatrix* B = omxInitMatrix(NULL, 0, 0, TRUE, result->currentState);
2000         omxCopyMatrix(B, matList[0]);
2001         omxResizeMatrix(A, B->rows, 1, FALSE);
2002
2003         /* Conformability Check! */
2004         if(B->cols != B->rows) {
2005                 char *errstr = (char*) calloc(250, sizeof(char));
2006                 sprintf(errstr, "Non-square matrix in eigenvalue decomposition.\n");
2007                 omxRaiseError(result->currentState, -1, errstr);
2008                 free(errstr);
2009                 omxFreeMatrixData(A);
2010                 omxFreeMatrixData(B);
2011                 return;
2012         }
2013
2014         if(result->cols != 1 || result->rows != A->rows)
2015                 omxResizeMatrix(result, B->rows, 1, FALSE);
2016
2017         char N = 'N';                                           // Indicators for BLAS
2018
2019         int One = 1;
2020         int lwork = 10*B->rows;
2021
2022         int info;
2023
2024         double *WR = (double*) malloc(B->cols * sizeof(double));
2025         double *VR = (double*) malloc(B->rows * B->cols * sizeof(double));
2026         double *work = (double*) malloc(lwork * sizeof(double));
2027
2028         F77_CALL(dgeev)(&N, &N, &(B->rows), B->data, &(B->leading), WR, A->data, NULL, &One, NULL, &One, work, &lwork, &info);
2029         if(info != 0) {
2030                 char *errstr = (char*) calloc(250, sizeof(char));
2031                 sprintf(errstr, "DGEEV returned %d in (real) eigenvalue decomposition:", info);
2032                 if(info > 0)
2033                         sprintf(errstr, "%s argument %d had an illegal value.  Post this to the OpenMx wiki.\n", errstr, info);
2034                 else
2035                         sprintf(errstr, "%s Unable to decompose matrix: Not of full rank.\n", errstr);
2036                 omxRaiseError(result->currentState, -1, errstr);
2037                 free(errstr);
2038                 goto ImagEigenValCleanup;
2039         }
2040
2041         // Calculate Eigenvalue modulus.
2042         for(int i = 0; i < result->rows; i++) {
2043                 double value = omxMatrixElement(A, i, 0);                                       // A[i] is the ith imaginary eigenvalue
2044                 value *= value;                                                                                         // Squared imaginary part
2045                 if(value > EPSILON) {
2046                         value = sqrt(WR[i] *WR[i] + value);                             // Sort by eigenvalue modulus
2047                         WR[i] = value;
2048                         WR[++i] = value;                                                                                // Conjugate pair.
2049                 } else {
2050                         WR[i] = fabs(WR[i]);
2051                 }
2052         }
2053
2054         result->colMajor = TRUE;
2055
2056         // Sort results
2057         omxSortHelper(WR, A, result);
2058
2059 ImagEigenValCleanup:
2060         omxFreeMatrixData(A);           // FIXME: State-keeping for algebras would save significant time in memory allocation/deallocation
2061         omxFreeMatrixData(B);
2062         omxMatrixLeadingLagging(result);
2063
2064         free(WR);
2065         free(VR);
2066         free(work);
2067 }
2068
2069 void omxImaginaryEigenvectors(omxMatrix** matList, int numArgs, omxMatrix* result)
2070 {
2071         omxMatrix* A = omxInitMatrix(NULL, 0, 0, TRUE, result->currentState);
2072         omxCopyMatrix(result, matList[0]);
2073         omxResizeMatrix(A, result->rows, result->cols, FALSE);
2074
2075         /* Conformability Check! */
2076         if(A->cols != A->rows) {
2077                 char *errstr = (char*) calloc(250, sizeof(char));
2078                 sprintf(errstr, "Non-square matrix in (imaginary) eigenvalue decomposition.\n");
2079                 omxRaiseError(result->currentState, -1, errstr);
2080                 free(errstr);
2081                 omxFreeMatrixData(A);
2082                 return;
2083         }
2084
2085         char N = 'N';                                           // Indicators for BLAS
2086         char V = 'V';                                           // Indicators for BLAS
2087
2088         int One = 1;
2089         int lwork = 10*A->rows;
2090
2091         int info;
2092
2093         double *WR = (double*) malloc(A->cols * sizeof(double));
2094         double *WI = (double*) malloc(A->cols * sizeof(double));
2095         double *work = (double*) malloc(lwork * sizeof(double));
2096
2097         if(result->rows != A->rows || result->cols != A->cols)
2098                 omxResizeMatrix(result, A->rows, A->cols, FALSE);
2099
2100         F77_CALL(dgeev)(&N, &V, &(result->rows), result->data, &(result->leading), WR, WI, NULL, &One, A->data, &(A->leading), work, &lwork, &info);
2101         if(info != 0) {
2102                 char *errstr = (char*) calloc(250, sizeof(char));
2103                 sprintf(errstr, "DGEEV returned %d in eigenvalue decomposition:", info);
2104                 if(info > 0)
2105                         sprintf(errstr, "%s argument %d had an illegal value.  Post this to the OpenMx wiki.\n", errstr, info);
2106                 else
2107                         sprintf(errstr, "%s Unable to decompose matrix: Not of full rank.\n", errstr);
2108                 omxRaiseError(result->currentState, -1, errstr);
2109                 free(errstr);
2110                 goto ImagEigenVecCleanup;
2111         }
2112
2113         // Filter real and imaginary eigenvectors.  Imaginary ones have a WI.
2114         for(int i = 0; i < result->cols; i++) {
2115                 if(WI[i] != 0) {                                // FIXME: Might need to be abs(WI[i] > EPSILON)
2116                         // memcpy(omxLocationOfMatrixElement(A, 0, i), omxLocationOfMatrixElement(A, 0, i+1), A->rows * sizeof(double));
2117                         for(int j = 0; j < result->rows; j++) {
2118                                 double value = omxMatrixElement(A, j, i+1);                     // Conjugate pair
2119                                 omxSetMatrixElement(A, j, i, value);                            // Positive first,
2120                                 omxSetMatrixElement(A, j, i+1, -value);                         // Negative second
2121                         }
2122                         WR[i] = sqrt(WR[i] *WR[i] + WI[i]*WI[i]);                               // Sort by eigenvalue modulus
2123                         WR[i+1] = WR[i];                                                                                // Identical--conjugate pair
2124                         i++;    // Skip the next one; we know it's the conjugate pair.
2125                 } else {                                                // If it's not imaginary, it's zero.
2126                         for(int j = 0; j < A->rows; j++) {
2127                                 omxSetMatrixElement(A, j, i, 0.0);
2128                         }
2129                         WR[i] = fabs(WR[i]);                                                                    // Modulus of a real is its absolute value
2130
2131                 }
2132         }
2133
2134         result->colMajor = TRUE;
2135
2136         omxSortHelper(WR, A, result);
2137
2138 ImagEigenVecCleanup:
2139         omxFreeMatrixData(A);                   // FIXME: State-keeping for algebras would save significant time in memory allocation/deallocation
2140         omxMatrixLeadingLagging(result);
2141
2142         free(WR);
2143         free(WI);
2144         free(work);
2145
2146 }
2147
2148 void omxSelectRows(omxMatrix** matList, int numArgs, omxMatrix* result)
2149 {
2150         omxMatrix* inMat = matList[0];
2151         omxMatrix* selector = matList[1];
2152
2153         int rows = inMat->rows;
2154     int selectLength = selector->rows * selector->cols;
2155     int toRemove[rows];
2156     int numRemoves = 0;
2157     
2158     if((selector->cols != 1) && selector->rows !=1) {
2159                 char *errstr = (char*) calloc(250, sizeof(char));
2160                 sprintf(errstr, "Selector must have a single row or a single column.\n");
2161         omxRaiseError(result->currentState, -1, errstr);
2162                 free(errstr);
2163                 return;
2164     }
2165
2166         if(selectLength != rows) {
2167                 char *errstr = (char*) calloc(250, sizeof(char));
2168                 sprintf(errstr, "Non-conformable matrices for row selection.\n");
2169         omxRaiseError(result->currentState, -1, errstr);
2170                 free(errstr);
2171                 return;
2172         }
2173         
2174         if(result->aliasedPtr != inMat) {
2175         omxAliasMatrix(result, inMat);
2176         }
2177         
2178     omxResetAliasedMatrix(result);
2179         
2180     for(int index = 0; index < selectLength; index++) {
2181         if(omxVectorElement(selector, index) == 0) {
2182             numRemoves++;
2183             toRemove[index] = 1;
2184         } else {
2185             toRemove[index] = 0;
2186         }
2187     }
2188     
2189     if(numRemoves >= rows) {
2190                 char *errstr = (char*) calloc(250, sizeof(char));
2191                 sprintf(errstr, "Attempted to select zero columns.\n");
2192         omxRaiseError(result->currentState, -1, errstr);
2193                 free(errstr);        
2194                 return;
2195     }
2196     
2197     int zeros[inMat->cols];
2198     memset(zeros, 0, sizeof(*zeros) * inMat->cols);
2199     omxRemoveRowsAndColumns(result, numRemoves, 0, toRemove, zeros);
2200
2201 }
2202
2203 void omxSelectCols(omxMatrix** matList, int numArgs, omxMatrix* result)
2204 {
2205         omxMatrix* inMat = matList[0];
2206         omxMatrix* selector = matList[1];
2207
2208         int cols = inMat->cols;
2209     int selectLength = selector->rows * selector->cols;
2210     int toRemove[cols];
2211     int numRemoves = 0;
2212
2213     if((selector->cols != 1) && selector->rows !=1) {
2214                 char *errstr = (char*) calloc(250, sizeof(char));
2215                 sprintf(errstr, "Selector must have a single row or a single column.\n");
2216         omxRaiseError(result->currentState, -1, errstr);
2217                 free(errstr);        
2218                 return;
2219     }
2220
2221         if(selectLength != cols) {
2222                 char *errstr = (char*) calloc(250, sizeof(char));
2223                 sprintf(errstr, "Non-conformable matrices for row selection.\n");
2224         omxRaiseError(result->currentState, -1, errstr);
2225                 free(errstr);
2226                 return;
2227         }
2228         
2229         if(result->aliasedPtr != inMat) {
2230         omxAliasMatrix(result, inMat);
2231         }
2232         
2233     omxResetAliasedMatrix(result);
2234         
2235     for(int index = 0; index < selectLength; index++) {
2236         if(omxVectorElement(selector, index) == 0) {
2237             numRemoves++;
2238             toRemove[index] = 1;
2239         } else {
2240             toRemove[index] = 0;
2241         }
2242     }
2243     
2244     if(numRemoves >= cols) {
2245                 char *errstr = (char*) calloc(250, sizeof(char));
2246                 sprintf(errstr, "Attempted to select zero columns.\n");
2247         omxRaiseError(result->currentState, -1, errstr);
2248                 free(errstr);        
2249                 return;
2250     }
2251     
2252     int zeros[inMat->rows];
2253     memset(zeros, 0, sizeof(*zeros) * inMat->rows);
2254     omxRemoveRowsAndColumns(result, 0, numRemoves, zeros, toRemove);
2255     
2256 }
2257
2258 void omxSelectRowsAndCols(omxMatrix** matList, int numArgs, omxMatrix* result)
2259 {
2260         omxMatrix* inMat = matList[0];
2261         omxMatrix* selector = matList[1];
2262
2263         int rows = inMat->rows;
2264         int cols = inMat->cols;
2265     int selectLength = selector->rows * selector->cols;
2266     int toRemove[cols];
2267     int numRemoves = 0;
2268
2269     if((selector->cols != 1) && selector->rows !=1) {
2270                 char *errstr = (char*) calloc(250, sizeof(char));
2271                 sprintf(errstr, "Selector must have a single row or a single column.\n");
2272         omxRaiseError(result->currentState, -1, errstr);
2273                 free(errstr);        
2274                 return;
2275     }
2276
2277         if(rows != cols) {
2278                 char *errstr = (char*) calloc(250, sizeof(char));
2279                 sprintf(errstr, "Can only select rows and columns from square matrices.\n");
2280         omxRaiseError(result->currentState, -1, errstr);
2281                 free(errstr);
2282                 return;
2283         }
2284
2285         if(selectLength != cols) {
2286                 char *errstr = (char*) calloc(250, sizeof(char));
2287                 sprintf(errstr, "Non-conformable matrices for row selection.\n");
2288         omxRaiseError(result->currentState, -1, errstr);
2289                 free(errstr);
2290                 return;
2291         }
2292         
2293         if(result->aliasedPtr != inMat) {
2294         omxAliasMatrix(result, inMat);
2295         }
2296         
2297     omxResetAliasedMatrix(result);
2298         
2299     for(int index = 0; index < selectLength; index++) {
2300         if(omxVectorElement(selector, index) == 0) {
2301             numRemoves++;
2302             toRemove[index] = 1;
2303         } else {
2304             toRemove[index] = 0;
2305         }
2306     }
2307     
2308     if(numRemoves >= cols) {
2309                 char *errstr = (char*) calloc(250, sizeof(char));
2310                 sprintf(errstr, "Attempted to select zero columns.\n");
2311         omxRaiseError(result->currentState, -1, errstr);
2312                 free(errstr);        
2313                 return;
2314     }
2315     
2316     omxRemoveRowsAndColumns(result, numRemoves, numRemoves, toRemove, toRemove);
2317
2318 }
2319
2320 void omxAddOwnTranspose(omxMatrix** matlist, int numArgs, omxMatrix* result) {
2321     omxMatrix* M = *matlist;
2322
2323     if(M->rows != M->cols || M->rows != result->cols || result->rows != result->cols) {
2324         char *errstr = (char*) calloc(250, sizeof(char));
2325         sprintf(errstr, "A + A^T attempted on asymmetric matrix.\n");
2326         omxRaiseError(result->currentState, -1, errstr);
2327         free(errstr);
2328                 return;
2329     }
2330     
2331     double total;
2332     
2333     for(int i = 0; i < result->rows; i++) {
2334         for(int j = 0; j < i; j++) {
2335             total = omxMatrixElement(M, i, j);
2336             total += omxMatrixElement(M, j, i);
2337             omxSetMatrixElement(result, i, j, total);
2338             omxSetMatrixElement(result, j, i, total);
2339         }
2340         omxSetMatrixElement(result, i, i, 2 * omxMatrixElement(M, i, i));
2341     }
2342     
2343 }
2344
2345 void omxCovToCor(omxMatrix** matList, int numArgs, omxMatrix* result)
2346 {
2347
2348     omxMatrix* inMat = matList[0];
2349     int rows = inMat->rows;
2350
2351         omxMatrix* intermediate;
2352
2353     if(inMat->rows != inMat->cols) {
2354         char *errstr = (char*) calloc(250, sizeof(char));
2355         sprintf(errstr, "cov2cor of non-square matrix cannot even be attempted\n");
2356         omxRaiseError(result->currentState, -1, errstr);
2357         free(errstr);
2358                 return;
2359         }
2360
2361         if(result->rows != rows || result->cols != rows) {
2362         if(OMX_DEBUG_ALGEBRA) { mxLog("ALGEBRA: cov2cor resizing result.");}
2363         omxResizeMatrix(result, rows, rows, FALSE);
2364         }
2365
2366     intermediate = omxInitTemporaryMatrix(NULL, 1, rows, TRUE, inMat->currentState);
2367
2368     for(int i = 0; i < rows; i++) {
2369         intermediate->data[i] = sqrt(1.0 / omxMatrixElement(inMat, i, i));
2370     }
2371
2372     if (inMat->colMajor) {
2373         for(int col = 0; col < rows; col++) {
2374             for(int row = 0; row < rows; row++) {
2375                 result->data[col * rows + row] = inMat->data[col * rows + row] * 
2376                     intermediate->data[row] * intermediate->data[col];
2377             }
2378         }
2379     } else {
2380         for(int col = 0; col < rows; col++) {
2381             for(int row = 0; row < rows; row++) {
2382                 result->data[col * rows + row] = inMat->data[row * rows + col] * 
2383                     intermediate->data[row] * intermediate->data[col];
2384             }
2385         }
2386     }
2387
2388     for(int i = 0; i < rows; i++) {
2389         result->data[i * rows + i] = 1.0;
2390     }
2391
2392     omxFreeAllMatrixData(intermediate);
2393 }
2394
2395 void omxCholesky(omxMatrix** matList, int numArgs, omxMatrix* result)
2396 {
2397         omxMatrix* inMat = matList[0];
2398
2399     int l = 0; char u = 'U';
2400         omxCopyMatrix(result, inMat);
2401         if(result->rows != result->cols) {
2402                 char *errstr = (char*) calloc(250, sizeof(char));
2403                 sprintf(errstr, "Cholesky decomposition of non-square matrix cannot even be attempted\n");
2404                 omxRaiseError(result->currentState, -1, errstr);
2405                 free(errstr);
2406                 return;
2407         }
2408
2409     F77_CALL(dpotrf)(&u, &(result->rows), result->data, &(result->cols), &l);
2410         if(l != 0) {
2411                 char *errstr = (char*) calloc(250, sizeof(char));
2412                 sprintf(errstr, "Attempted to Cholesky decompose non-invertable matrix.\n");
2413                 omxRaiseError(result->currentState, -1, errstr);
2414                 free(errstr);
2415                 return;
2416         } else {
2417             for(int i = 0; i < result->rows; i++) {
2418                         for(int j = 0; j < i; j++) {
2419                 omxSetMatrixElement(result, i, j, 0.0);
2420                         }
2421                 }
2422         }
2423 }
2424
2425 void omxVechToMatrix(omxMatrix** matList, int numArgs, omxMatrix* result) {
2426
2427         omxMatrix *inMat = matList[0];
2428         
2429         int dim = (inMat->cols > inMat->rows) ? inMat->cols : inMat->rows;
2430
2431         int size = sqrt(2.0 * dim + 0.25) - 0.5;
2432
2433         int counter = 0;
2434
2435     if(inMat->cols > 1 && inMat->rows > 1) {
2436         char *errstr = (char*) calloc(250, sizeof(char));
2437         sprintf(errstr, "vech2full input has %d rows and %d columns\n", inMat->rows, inMat->cols);
2438         omxRaiseError(result->currentState, -1, errstr);
2439         free(errstr);
2440                 return;
2441         }
2442
2443         /* Consistency check: */
2444         if(result->rows != size || result->cols != size) {
2445                 omxResizeMatrix(result, size, size, FALSE);
2446         }
2447
2448         for(int i = 0; i < size; i++) {
2449                 for(int j = i; j < size; j++) {
2450
2451                         double next = omxVectorElement(inMat, counter);
2452
2453                         omxSetMatrixElement(result, i, j, next);
2454                         omxSetMatrixElement(result, j, i, next);
2455
2456                         counter++;
2457                 }
2458         }
2459
2460 }
2461
2462
2463 void omxVechsToMatrix(omxMatrix** matList, int numArgs, omxMatrix* result) {
2464         omxMatrix *inMat = matList[0];
2465         
2466         int dim = (inMat->cols > inMat->rows) ? inMat->cols : inMat->rows;
2467         
2468         int size = sqrt(2.0 * dim + 0.25) + 0.5; //note the plus 0.5
2469
2470         int counter = 0;
2471
2472     if(inMat->cols > 1 && inMat->rows > 1) {
2473         char *errstr = (char*) calloc(250, sizeof(char));
2474         sprintf(errstr, "vechs2full input has %d rows and %d columns\n", inMat->rows, inMat->cols);
2475         omxRaiseError(result->currentState, -1, errstr);
2476         free(errstr);
2477                 return;
2478         }
2479
2480         /* Consistency check: */
2481         if(result->rows != size || result->cols != size) {
2482                 omxResizeMatrix(result, size, size, FALSE);
2483         }
2484
2485         for(int i = 0; i < size; i++) {
2486
2487                 omxSetMatrixElement(result, i, i, 0.0);
2488
2489                 for(int j = i + 1; j < size; j++) {
2490
2491                         double next = omxVectorElement(inMat, counter);
2492
2493                         omxSetMatrixElement(result, i, j, next);
2494                         omxSetMatrixElement(result, j, i, next);
2495
2496                         counter++;
2497                 }
2498         }
2499
2500 }
2501
2502