/*
*-----------------------------------------------------------------------------
* funct: mat_mul
* desct: multiplication of two matrices
* given: A, B = compatible matrices to be multiplied
* retrn: NULL if malloc() fails
* else allocated matrix of A * B
* comen:
*-----------------------------------------------------------------------------
*/
MATRIX mat_mul( MATRIX A, MATRIX B , MATRIX C)
{
int i, j, k;
#ifdef CONFORM_CHECK
if(MatCol(A)!=MatRow(B))
{
error("\nUnconformable matrices in routine mat_mul(): Col(A)!=Row(B) (%d/%d)\n", MatCol(A),MatRow(B));
}
if(MatRow(A)!=MatRow(C))
{
error("\nUnconformable matrices in routine mat_mul(): Row(A)!=Row(C) (%d/%d)\n", MatRow(A), MatRow(C));
}
if(MatCol(B)!=MatCol(C))
{
error("\nUnconformable matrices in routine mat_mul(): Col(B)!=Col(C) (%d/%d)\n", MatCol(B), MatCol(C));
}
#endif
for (i=0; i<MatRow(A); i++)
{
for (j=0; j<MatCol(B); j++)
{
for (k=0, C[i][j]=0.0; k<MatCol(A); k++)
{
C[i][j] += A[i][k] * B[k][j];
}
}
}
return (C);
}
/*
*-----------------------------------------------------------------------------
* funct: mat_durbin
* desct: Levinson-Durbin algorithm
*
* This function solve the linear eqns Ax = B:
*
* | v0 v1 v2 .. vn-1 | | a1 | | v1 |
* | v1 v0 v1 .. vn-2 | | a2 | | v2 |
* | v2 v1 v0 .. vn-3 | | a3 | = | .. |
* | ... | | .. | | .. |
* | vn-1 vn-2 .. .. v0 | | an | | vn |
*
* where A is a symmetric Toeplitz matrix and B
* in the above format (related to A)
*
* given: R = autocorrelated matrix (v0, v1, ... vn) (dim (n+1) x 1)
* retrn: x (of Ax = B)
*-----------------------------------------------------------------------------
*/
MATRIX mat_durbin(MATRIX R, MATRIX X)
{
int i, i1, j, ji, p;
MATRIX W, E, K, A;
// if dimensions of X is wrong
p = MatRow(R) - 1;
if ( MatRow(X) != p || MatCol(X) != 1 ) {
printf("mat_durbin error: incompatible output matrix size\n");
_exit(-1);
// if dimensions of X is correct
} else {
if ( ( W = mat_creat( p+2, 1, UNDEFINED ) ) == NULL )
return (NULL);
if ( ( E = mat_creat( p+2, 1, UNDEFINED ) ) == NULL )
return (NULL);
if ( ( K = mat_creat( p+2, 1, UNDEFINED ) ) == NULL )
return (NULL);
if ( ( A = mat_creat( p+2, p+2, UNDEFINED ) ) == NULL )
return (NULL);
W[0][0] = R[1][0];
E[0][0] = R[0][0];
for (i=1; i<=p; i++) {
K[i][0] = W[i-1][0] / E[i-1][0];
E[i][0] = E[i-1][0] * (1.0 - K[i][0] * K[i][0]);
A[i][i] = -K[i][0];
i1 = i-1;
if (i1 >= 1) {
for (j=1; j<=i1; j++) {
ji = i - j;
A[j][i] = A[j][i1] - K[i][0] * A[ji][i1];
}
}
if (i != p) {
W[i][0] = R[i+1][0];
for (j=1; j<=i; j++)
W[i][0] += A[j][i] * R[i-j+1][0];
}
}
for (i=0; i<p; i++) {
X[i][0] = -A[i+1][p];
}
}
mat_free( A );
mat_free( W );
mat_free( K );
mat_free( E );
return(X);
}
MATRIX mat_concat(MATRIX A, MATRIX B, int dim)
{
int i, j, m, n, o, p;
MATRIX result;
if(A==NULL)
{
return mat_copy(B, NULL);
}
else
{
m = MatCol(A);
n = MatRow(A);
}
if(B==NULL)
{
return mat_copy(A, NULL);
}
else
{
o = MatCol(B);
p = MatRow(B);
}
if((dim==ROWS)&&((m==o) ||!((m==0)&&(o==0))))
{
if((result = mat_creat(n+p, m, UNDEFINED))==NULL) return NULL;
#pragma omp parallel for private(j)
for(i=0; i<n; ++i)
{
for(j=0; j<m; ++j)
{
result[i][j] = A[i][j];
}
}
#pragma omp parallel for private(j)
for(i=0; i<p; ++i)
{
for(j=0; j<m; ++j)
{
result[i+n][j] = B[i][j];
}
}
return result;
}
if((dim==COLS)&&((n==p) ||!((n==0)&&(p==0))))
{
if((result = mat_creat(n, m+o, UNDEFINED))==NULL) return NULL;
#pragma omp parallel for private(j)
for(i=0; i<n; ++i)
{
for(j=0; j<m; ++j) result[i][j] = A[i][j];
for(j=0; j<o; ++j) result[i][j+m] = B[i][j];
}
return result;
}
return mat_error(MAT_SIZEMISMATCH);
}
/*
*-----------------------------------------------------------------------------
* funct: mat_det
* desct: find determinant
* given: A = matrix
* retrn: the determinant of A
* comen:
*-----------------------------------------------------------------------------
*/
double mat_det( MATRIX a )
{
MATRIX A, P;
int j;
int i, n;
double result;
n = MatRow(a);
A = mat_creat(MatRow(a), MatCol(a), UNDEFINED);
A = mat_copy(a, A);
P = mat_creat(n, 1, UNDEFINED);
/*
* take a LUP-decomposition
*/
i = mat_lu(A, P);
switch (i)
{
/*
* case for singular matrix
*/
case -1:
result = 0.0;
break;
/*
* normal case: |A| = |L||U||P|
* |L| = 1,
* |U| = multiplication of the diagonal
* |P| = +-1
*/
default:
result = 1.0;
for (j=0; j<MatRow(A); j++)
{
result *= A[(int)P[j][0]][j];
}
result *= signa[i%2];
break;
}
mat_free(A);
mat_free(P);
return (result);
}
/*
*-----------------------------------------------------------------------------
* funct: mat_inv
* desct: find inverse of a matrix
* given: a = square matrix a
* retrn: square matrix Inverse(A)
* NULL = fails, singular matrix, or malloc() fails
* 1 = success
*-----------------------------------------------------------------------------
*/
MATRIX mat_inv(MATRIX a, MATRIX C)
{
MATRIX A, B, P;
int i, n;
n = MatCol(a);
if ( ( A = mat_creat( n, n, UNDEFINED ) ) == NULL )
return (NULL);
mat_copy(a,A);
if ( ( B = mat_creat( n, 1, UNDEFINED ) ) == NULL )
return (NULL);
if ( ( P = mat_creat( n, 1, UNDEFINED ) ) == NULL )
return (NULL);
// if dimensions of C is wrong
if ( MatRow(a) != MatRow(C) || MatCol(a) != MatCol(C) ) {
printf("mat_inv error: incompatible output matrix size\n");
_exit(-1);
// if dimensions of C is correct
} else {
/*
* - LU-decomposition -
* also check for singular matrix
*/
if (mat_lu(A, P) == -1) {
mat_free(A);
mat_free(B);
mat_free(C);
mat_free(P);
printf("mat_inv error: failed to invert\n");
return (NULL);
}
for (i=0; i<n; i++) {
mat_fill(B, ZERO_MATRIX);
B[i][0] = 1.0;
mat_backsubs1( A, B, C, P, i );
}
}
mat_free(A);
mat_free(B);
mat_free(P);
if (C==NULL) {
printf("mat_inv error: failed to invert\n");
return(NULL);
} else {
return (C);
}
}
MATRIX mat_transmul(MATRIX A,MATRIX B, MATRIX C)
{
/* computes C = A * B' */
int i, j, k;
for (i=0; i<MatRow(A); i++)
for (j=0; j<MatRow(B); j++)
for (k=0, C[i][j]=0.0; k<MatCol(A); k++) {
C[i][j] += A[i][k] * B[j][k];
}
return(C);
}
MATRIX mat_xjoin(MATRIX A11, MATRIX A12, MATRIX A21, MATRIX A22, MATRIX result)
{
int i, j, m, n;
m = MatCol(A11)+MatCol(A12);
n = MatRow(A11)+MatRow(A21);
if(result== NULL) if((result = mat_creat(m, n, UNDEFINED))==NULL)
return mat_error(MAT_MALLOC);
#pragma omp parallel for private(j)
for(i=0; i<MatRow(A11); ++i)
for(j=0; j<MatCol(A11); ++j)
{
result[i][j] = A11[i][j];
}
#pragma omp parallel for private(j)
for(i=0; i<MatRow(A12); ++i)
for(j=0; j<MatCol(A12); ++j)
{
result[i][j+MatCol(A11)] = A12[i][j];
}
#pragma omp parallel for private(j)
for(i=0; i<MatRow(A21); ++i)
for(j=0; j<MatCol(A21); ++j)
{
result[i+MatRow(A21)][j] = A21[i][j];
}
#pragma omp parallel for private(j)
for(i=0; i<MatRow(A22); ++i)
for(j=0; j<MatCol(A22); ++j)
{
result[i+MatRow(A11)][j+MatCol(A22)] = A22[i][j];
}
return result;
}
void EcefToEnu(MATRIX outputVector, MATRIX inputVector, MATRIX position)
{
int i;
double lat, lon;
MATRIX C, ned, ref_position;
MATRIX position_copy, delta_pos;
C = mat_creat(3,3,ZERO_MATRIX);
ref_position = mat_creat(3,1,ZERO_MATRIX);
delta_pos = mat_creat(3,1,ZERO_MATRIX);
position_copy = mat_creat(MatRow(position),MatCol(position),ZERO_MATRIX);
mat_copy(position, position_copy);
lat = position[0][0];
lon = position[1][0];
LatLonAltToEcef(ref_position,position_copy);
mat_sub(inputVector,ref_position,delta_pos);
C[0][0] = -sin(lon);
C[0][1] = cos(lon);
C[0][2] = 0;
C[1][0] = -sin(lat)*cos(lon);
C[1][1] = -sin(lat)*sin(lon);
C[1][2] = cos(lat);
C[2][0] = cos(lat)*cos(lon);
C[2][1] = cos(lat)*sin(lon);
C[2][2] = sin(lat);
ned = mat_creat(MatRow(C),MatCol(delta_pos),ZERO_MATRIX);
mat_mul(C,delta_pos,ned);
for (i = 0; i < 3; i++)
{
outputVector[i][0] = ned[i][0];
}
mat_free(ned);
mat_free(C);
mat_free(delta_pos);
mat_free(ref_position);
mat_free(position_copy);
}
/*
*-----------------------------------------------------------------------------
* funct: mat_free
* desct: free an allocated matrix
* given: A = matrix
* retrn: nothing <actually 0 = NULL A passed, 1 = normal exit>
*-----------------------------------------------------------------------------
*/
int mat_free( MATRIX A )
{
int i;
if (A==NULL)
{
REprintf("\nAttempting to free a non-existent matrix in mat_free()\n");
return (0);
}
for (i=0; i<MatRow(A); i++)
{
if((A[i]==NULL))
{
REprintf("\nAttempting to free a non-existent matrix row in mat_free()\n");
return (0);
}
else
{
free(A[i]);
}
}
free(Mathead(A));
return (1);
}
/*
*-----------------------------------------------------------------------------
* funct: mat_lsolve
* desct: solve linear equations
* given: a = square matrix A
* b = column matrix B
* retrn: column matrix X (of AX = B)
*-----------------------------------------------------------------------------
*/
MATRIX mat_lsolve(MATRIX a,MATRIX b,MATRIX X)
{
MATRIX A, B, P;
int n;
n = MatCol(a);
if ( ( A = mat_creat(n, n, UNDEFINED) ) == NULL )
return (NULL);
if ( ( B = mat_creat(n, 1, UNDEFINED) ) == NULL )
return (NULL);
mat_copy(a,A);
mat_copy(b,B);
if ( ( P = mat_creat(n, 1, UNDEFINED) ) == NULL )
return (NULL);
// if dimensions of C is wrong
if ( MatRow(X) != n || MatCol(X) != 1 ) {
printf("mat_lsolve error: incompatible output matrix size\n");
_exit(-1);
// if dimensions of C is correct
} else {
mat_lu( A, P );
mat_backsubs1( A, B, X, P, 0 );
}
mat_free(A);
mat_free(B);
mat_free(P);
return(X);
}
MATSTACK mat_eig_sym(MATRIX symmat, MATSTACK result)
{
int m, n;
MATRIX im, tmp_result0 = NULL, tmp_result1 = NULL;
INT_VECTOR indcs = NULL;
MATSTACK tmp = NULL;
m = MatCol(symmat);
n = MatRow(symmat);
if(m!=n) mat_error(MAT_SIZEMISMATCH);
if(result==NULL)
{
if ((result = matstack_creat(2)) == NULL)
return matstack_error(MATSTACK_MALLOC);
result[0] = NULL;
result[1] = NULL;
}
im = mat_creat(m, 1, UNDEFINED);
tmp_result0 = mat_creat(m, 1, UNDEFINED);
tmp_result1 = mat_copy(symmat, tmp_result1);
mat_tred2(tmp_result1, tmp_result0, im);
mat_tqli(tmp_result0, im, tmp_result1);
tmp = mat_qsort(tmp_result0, ROWS, tmp);
result[0] = mat_copy(tmp[0], result[0]);
indcs = mat_2int_vec(tmp[1]);
result[1] = mat_get_sub_matrix_from_cols(tmp_result1, indcs, result[1]);
int_vec_free(indcs);
mat_free(im);
mat_free(tmp_result0);
mat_free(tmp_result1);
return result;
}
MATRIX mat_fill_type(MATRIX A, int type)
{
int i, j, m, n;
m = MatCol(A);
n = MatRow(A);
switch(type)
{
case UNDEFINED:
break;
case ZERO_MATRIX:
case UNIT_MATRIX:
#pragma omp parallel for private(j)
for(i=0; i<n; ++i)
for(j=0; j<m; ++j)
{
if(type==UNIT_MATRIX)
{
if(i==j)
{
A[i][j] = 1.0;
continue;
}
}
A[i][j] = 0.0;
}
break;
case ONES_MATRIX:
#pragma omp parallel for private(j)
for(i=0; i<n; ++i)
for(j=0; j<m; ++j) A[i][j] = 1.0;
break;
}
return A;
}
MATRIX mat_fill( MATRIX A, int type )
{
int i, j;
switch (type)
{
case UNDEFINED:
break;
case ZERO_MATRIX:
case UNIT_MATRIX:
for (i=0; i<MatRow(A); i++)
{
for (j=0; j<MatCol(A); j++)
{
if (type == UNIT_MATRIX)
{
if (i==j)
{
A[i][j] = 1.0;
continue;
}
}
A[i][j] = 0.0;
}
}
break;
}
return (A);
}
MATSTACK mat_corcol(MATRIX data)
{
mtype x;
MATRIX stddev;
int i, j, j1, j2, m, n;
MATSTACK corr = matstack_creat(3);
m = MatCol(data);
n = MatRow(data);
corr[0] = mat_creat(1, m, ZERO_MATRIX);
stddev = mat_creat(1, m, ZERO_MATRIX);
corr[1] = mat_creat(m, m, ZERO_MATRIX);
corr[2] = mat_creat(n, m, ZERO_MATRIX);
for(j=0; j<m; ++j)
{
corr[0][0][j] = 0.0;
for(i=0; i<n; ++i)
{
corr[0][0][j] += data[i][j];
}
corr[0][0][j] /= (mtype)n;
}
for(j=0; j<m; ++j)
{
stddev[0][j] = 0.0;
for(i=0; i<n; ++i)
{
stddev[0][j] += ((data[i][j] - corr[0][0][j])*(data[i][j] - corr[0][0][j]));
}
stddev[0][j] /= (mtype)n;
stddev[0][j] = (mtype)sqrt(stddev[0][j]);
if(stddev[0][j]<=Eps) stddev[0][j] = 1.0;
}
for(i=0; i<n; ++i)
{
for(j=0; j<m; ++j)
{
corr[2][i][j] = data[i][j]-corr[0][0][j];
x = (mtype)sqrt((mtype)n);
x *= stddev[0][j];
corr[2][i][j] = corr[2][i][j]/x;
}
}
for(j1=0; j1<m-1; ++j1)
{
corr[1][j1][j1] = 1.0;
for(j2=j1+1; j2<m; ++j2)
{
corr[1][j1][j2] = 0.0;
for(i=0; i<n; ++i)
{
corr[1][j1][j2] += (corr[2][i][j1]*corr[2][i][j2]);
}
corr[1][j2][j1] = corr[1][j1][j2];
}
}
corr[1][m-1][m-1] = 1.0;
mat_free(stddev);
return corr;
}
MATRIX mat_pick_col(MATRIX A, int c, MATRIX result)
{
int n, i;
n = MatRow(A);
if(result==NULL) if((result = mat_creat(n, 1, UNDEFINED))==NULL) mat_error(MAT_MALLOC);
for(i=0; i<n; ++i) result[i][0] = A[i][c];
return result;
}
MATRIX mat_div_dot(MATRIX A, MATRIX B, MATRIX result)
{
int i, j, m, n, o, p;
m = MatCol(A);
n = MatRow(A);
o = MatCol(B);
p = MatRow(B);
if(result==NULL) if((result = mat_creat(MatRow(A), MatCol(A), UNDEFINED))==NULL)
return mat_error(MAT_MALLOC);
if(o==m &&p==n)
{
#pragma omp parallel for private(j)
for(i=0; i<n; ++i)
{
for(j=0; j<m; ++j)
{
result[i][j] = A[i][j]/B[i][j];
}
}
}
else if(o==1 && p!=1)
{
#pragma omp parallel for private(j)
for(i=0; i<n; ++i)
{
for(j=0; j<m; ++j)
{
result[i][j] = A[i][j]/B[i][0];
}
}
}
else if(p==1 && o!=1)
{
#pragma omp parallel for private(j)
for(i=0; i<n; ++i)
{
for(j=0; j<m; ++j)
{
result[i][j] = A[i][j]/B[0][j];
}
}
}
else gen_error(GEN_SIZEMISMATCH);
return result;
}
/*
*-----------------------------------------------------------------------------
* funct: mat_SymToeplz
* desct: create a n x n symmetric Toeplitz matrix from
* a n x 1 correlation matrix
* given: R = correlation matrix (n x 1)
* retrn: the symmetric Toeplitz matrix
*-----------------------------------------------------------------------------
*/
MATRIX mat_SymToeplz(MATRIX R,MATRIX T)
{
int i, j, n;
n = MatRow(R);
// if dimensions of T is wrong
if ( n != MatRow(T) || n != MatCol(T) ) {
printf("mat_SymToeplz error: incompatible output matrix size\n");
_exit(-1);
// if dimensions of T is correct
} else {
for (i=0; i<n; i++)
for (j=0; j<n; j++) {
T[i][j] = R[abs(i-j)][0];
}
}
return(T);
}
MATRIX mat_colcopy(MATRIX A, int cola, int colb, MATRIX result)
{
int i, n;
n = MatRow(result);
for(i=0; i<n; ++i)
{
result[i][colb] = A[i][cola];
}
return result;
}
mtype mat_sum(MATRIX A)
{
int i, j, m, n;
mtype mn = 0.0;
m = MatCol(A);
n = MatRow(A);
for (i=0; i<n; ++i)
for(j=0; j<m; ++j) mn += A[i][j];
return mn;
}
MATRIX mat_round (MATRIX X, MATRIX C)
{
int i, j;
// if dimensions of C is wrong
if ( MatRow(C) != MatRow(X) || MatCol(C) != MatCol(X) ) {
printf("mat_round error: incompatible output matrix size\n");
_exit(-1);
// if dimensions of C is correct
} else {
for (i=0; i<MatRow(X); i++) {
for (j=0; j<MatCol(X); j++) {
//temp = (int) (X[i][j]+.5*);
C[i][j] =floor(X[i][j]+.5);
}
}
}
return(C);
}
MATRIX mat_fill(MATRIX A, mtype val)
{
int i, j, m, n;
m = MatCol(A);
n = MatRow(A);
#pragma omp parallel for private(j)
for(i=0; i<n; ++i)
for(j=0; j<m; ++j)
A[i][j] = val;
return A;
}
MATRIX mat_colcopy1(MATRIX A,MATRIX B,int cola,int colb)
{
int i, n;
n = MatRow(A);
for (i=0; i<n; i++)
{
A[i][cola] = B[i][colb];
}
return (A);
}
double mat_diagmul(MATRIX A)
{
int i;
double result = 1.0;
for (i=0; i<MatRow(A); i++)
{
result *= A[i][i];
}
return (result);
}
MATRIX mat_mymul4(MATRIX A,MATRIX B, MATRIX C, short m)
{
/* Note: This function finds C = A * B' */
int i, j, k;
// if dimensions of C is wrong
//if ( MatRow(C) != MatRow(A) || MatCol(C) != MatCol(B) ) {
// printf("mat_mul error: incompatible output matrix size\n");
// _exit(-1);
// if dimensions of C is correct
//} else {
for (i=0; i<MatRow(A); i++)
for (j=0; j<MatRow(B); j++)
for (k=0, C[i][j]=0.0; k<MatCol(A)-m; k++) {
C[i][j] += A[i][k] * B[j][k];
}
//}
return(C);
}
// takes the norm of a single column
double mat_norm (MATRIX X, int column)
{
int i;
double tot, norm;
tot=0;
for (i=0; i<MatRow(X); i++) {
tot = tot + (X[i][column-1])*(X[i][column-1]);
}
norm=sqrt(tot);
return (norm);
}
int mat_fgetmat(MATRIX A, MAT_FILEPOINTER fp)
{
int i, j, k=0, m, n;
m = MatCol(A);
n = MatRow(A);
for(i=0; i<n; ++i)
#if mtype_n == 0
for(j=0; j<m; ++j) k += fscanf(fp, "%f", &A[i][j]);
#elif mtype_n == 1
for(j=0; j<m; j++) k += fscanf(fp, "%lf", &A[i][j]);
#endif
return k;
}
// Calculates the dot product of X and Y
double mat_dot (MATRIX X, MATRIX Y)
{
int i,j;
double dotProduct;
dotProduct=0;
for(i=0;i<MatRow(X);i++) {
for(j=0;j<MatCol(X);j++) {
dotProduct+=(X[i][j]*Y[i][j]);
}
}
return(fabs(dotProduct));
}
/*
*-----------------------------------------------------------------------------
* funct: mat_free
* desct: free an allocated matrix
* given: A = matrix
* retrn: nothing <actually 0 = NULL A passed, 1 = normal exit>
*-----------------------------------------------------------------------------
*/
int mat_free(MATRIX A)
{
int i;
if (A == NULL)
return (0);
for (i=0; i<MatRow(A); i++)
{
free( A[i] );
}
free( Mathead(A) );
return (1);
}
MATRIX mat_get_sub_matrix_from_cols(MATRIX A, INT_VECTOR indices, MATRIX result)
{
int i, j, k, n;
k = MatRow(A);
n = Int_VecLen(indices);
if(result==NULL) if((result = mat_creat(k, n, UNDEFINED))==NULL)
return mat_error(MAT_MALLOC);
for(i=0; i<n; ++i)
{
for(j=0; j<k; ++j) result[j][i] = A[j][indices[i]];
}
return result;
}
/*
*-----------------------------------------------------------------------------
* funct: mat_copy
* desct: duplicate a matrix
* given: A = matrix to duplicated
* retrn: C = A
* comen:
*-----------------------------------------------------------------------------
*/
MATRIX mat_copy( MATRIX A, MATRIX C )
{
int i, j;
for (i=0; i<MatRow(A); i++)
{
for (j=0; j<MatCol(A); j++)
{
C[i][j] = A[i][j];
}
}
return (C);
}
