Page MenuHomec4science
Files F122428797

spm_matfuns.c
No OneTemporary
Actions

File Metadata

Created
Thu, Jul 17, 19:39

spm_matfuns.c
View Options

/*
* $Id: spm_matfuns.c 4452 2011-09-02 10:45:26Z guillaume $
*/
#include <math.h>
int gaussj(double *a, int n, double *b, int m)
{
int icol = 0, ipiv[64], irow = 0, i, j, k, l, indxc[64], indxr[64], ll;
double pivinv, big, dum;
if (n>64) return(1);
for (j=0; j<n; ++j)
ipiv[j] = 0;
for (i=0; i<n; ++i)
{
big = 0.0;
for (j=0; j<n; ++j)
if (ipiv[j] != 1)
for (k=0; k<n; ++k)
{
if (ipiv[k] == 0)
{
if (fabs(a[j + k * n]) >= big)
{
big = fabs(a[j + k * n]);
irow = j;
icol = k;
}
}
else if (ipiv[k] > 1)
return(1);
}
++ipiv[icol];
if (irow != icol)
{
for (l=0; l<n; ++l)
{
dum = a[irow + l * n];
a[irow + l * n] = a[icol + l * n];
a[icol + l * n] = dum;
}
for (l=0; l<m; ++l)
{
dum = b[irow + l * n];
b[irow + l * n] = b[icol + l * n];
b[icol + l * n] = dum;
}
}
indxr[i] = irow;
indxc[i] = icol;
if (a[icol + icol * n] == 0.0)
return(1);
pivinv = 1.0 / a[icol + icol * n];
a[icol + icol * n] = 1.0;
for (l=0; l<n; ++l)
a[icol + l * n] *= pivinv;
for (l=0; l<m; ++l)
b[icol + l * n] *= pivinv;
for (ll=0; ll<n; ++ll)
if (ll != icol)
{
dum = a[ll + icol * n];
a[ll + icol * n] = 0.0;
for (l=0; l<n; ++l)
a[ll + l * n] -= a[icol + l * n] * dum;
for (l=0; l<m; ++l)
b[ll + l * n] -= b[icol + l * n] * dum;
}
}
for (l=n-1; l>=0; --l)
if (indxr[l] != indxc[l])
for (k=0; k<n; ++k)
{
dum = a[k + indxr[l] * n];
a[k + indxr[l] * n] = a[k + indxc[l] * n];
a[k + indxc[l] * n] = dum;
}
return(0);
}
/* M = MG\MF */
int AbackslashB(double *MG, double *MF, double *M)
{
int i;
double IMG[16];
for(i=0; i<16; i++)
{
IMG[i] = MG[i];
M[i] = MF[i];
}
if (gaussj(IMG, 4, M, 4)) return(1);
return(0);
}
void MtimesX(double *M, double *x, double *y)
{
y[0] = M[0+4*0]*x[0] + M[0+4*1]*x[1] + M[0+4*2]*x[2] + M[0+4*3];
y[1] = M[1+4*0]*x[0] + M[1+4*1]*x[1] + M[1+4*2]*x[2] + M[1+4*3];
y[2] = M[2+4*0]*x[0] + M[2+4*1]*x[1] + M[2+4*2]*x[2] + M[2+4*3];
}
/*
void mexFunction(int nlhs, Matrix *plhs[], int nrhs, Matrix *prhs[])
{
unsigned int n, m, i;
double *MG, *MF, *M, tmp[16];
if (nrhs == 0) mexErrMsgTxt("usage: M=leftdiv(MG,MF)");
if (nrhs != 2) mexErrMsgTxt("leftdiv: 2 input arguments required");
if (nlhs > 1) mexErrMsgTxt("leftdiv: only 1 output argument required");
if (!mxIsNumeric(prhs[0]) || mxIsComplex(prhs[0]) || !mxIsFull(prhs[0]) || !mxIsDouble(prhs[0]))
mexErrMsgTxt("leftdiv: MG must be numeric, real, full and double");
if (!mxIsNumeric(prhs[1]) || mxIsComplex(prhs[1]) || !mxIsFull(prhs[1]) || !mxIsDouble(prhs[1]))
mexErrMsgTxt("leftdiv: MF must be numeric, real, full and double");
if (mxGetM(prhs[0])!=4 || mxGetN(prhs[0])!=4 || mxGetM(prhs[1])!=4 || mxGetN(prhs[1])!=4)
mexErrMsgTxt("leftdiv: all matrices must be 4x4");
MG = mxGetPr(prhs[0]);
MF = mxGetPr(prhs[1]);
plhs[0] = mxCreateFull(4,4, REAL);
M = mxGetPr(plhs[0]);
AbackslashB(MG, MF, M);
}
void mexFunction(int nlhs, Matrix *plhs[], int nrhs, Matrix *prhs[])
{
unsigned int n, m, i;
double *X, *A, *B, *IA;
if (nrhs == 0) mexErrMsgTxt("usage: [X, IB]=leftdiv(B,C)");
if (nrhs != 2) mexErrMsgTxt("leftdiv: 2 input arguments required");
if (nlhs > 2) mexErrMsgTxt("leftdiv: only 2 output arguments required");
if (!mxIsNumeric(prhs[0]) || mxIsComplex(prhs[0]) || !mxIsFull(prhs[0]) || !mxIsDouble(prhs[0]))
mexErrMsgTxt("leftdiv: A must be numeric, real, full and double");
A = mxGetPr(prhs[0]);
n = mxGetM(prhs[0]);
if (mxGetN(prhs[0]) != n)
mexErrMsgTxt("leftdiv: A has incompatible no of columns");
if (!mxIsNumeric(prhs[1]) || mxIsComplex(prhs[1]) || !mxIsFull(prhs[1]) || !mxIsDouble(prhs[1]))
mexErrMsgTxt("leftdiv: B must be numeric, real, full and double");
B = mxGetPr(prhs[1]);
if (mxGetM(prhs[1]) != n)
mexErrMsgTxt("leftdiv: B has incompatible m dimension");
m = mxGetN(prhs[1]);
plhs[0] = mxCreateFull(n,m, REAL);
X = mxGetPr(plhs[0]);
plhs[1] = mxCreateFull(n,n, REAL);
IA = mxGetPr(plhs[1]);
for(i=0; i<m*n; i++) X[i] = B[i];
for(i=0; i<n*n; i++) IA[i] = A[i];
(void)gaussj(IA,m,X,n);
}
*/

Event Timeline

c4science · Help