[37] | 1 | #include "mex.h" |
---|
| 2 | #include <math.h> |
---|
| 3 | #include <stdlib.h> |
---|
| 4 | #include <malloc.h> |
---|
| 5 | #include <string.h> |
---|
| 6 | |
---|
| 7 | //void ludcmp(double **a, int n, int *indx, float *d) |
---|
| 8 | void ludcmp(double **a, int n, double *d, double *vv) |
---|
| 9 | { |
---|
| 10 | int i,imax,j,k; |
---|
| 11 | double big,dum,sum,temp; |
---|
| 12 | |
---|
| 13 | *d=1.0; |
---|
| 14 | for (i=1;i<=n;i++) { |
---|
| 15 | big=0.0; |
---|
| 16 | for (j=1;j<=n;j++) |
---|
| 17 | if ((temp=fabs(a[i][j])) > big) big=temp; |
---|
| 18 | if (big == 0.0) mexErrMsgTxt("Singular matrix in routine ludcmp"); |
---|
| 19 | vv[i]=1.0/big; |
---|
| 20 | } |
---|
| 21 | for (j=1;j<=n;j++) { |
---|
| 22 | for (i=1;i<j;i++) { |
---|
| 23 | sum=a[i][j]; |
---|
| 24 | for (k=1;k<i;k++) sum -= a[i][k]*a[k][j]; |
---|
| 25 | a[i][j]=sum; |
---|
| 26 | } |
---|
| 27 | big=0.0; |
---|
| 28 | for (i=j;i<=n;i++) { |
---|
| 29 | sum=a[i][j]; |
---|
| 30 | for (k=1;k<j;k++) |
---|
| 31 | sum -= a[i][k]*a[k][j]; |
---|
| 32 | a[i][j]=sum; |
---|
| 33 | if ( (dum=vv[i]*fabs(sum)) >= big) { |
---|
| 34 | big=dum; |
---|
| 35 | imax=i; |
---|
| 36 | } |
---|
| 37 | } |
---|
| 38 | if (j != imax) { |
---|
| 39 | for (k=1;k<=n;k++) { |
---|
| 40 | dum=a[imax][k]; |
---|
| 41 | a[imax][k]=a[j][k]; |
---|
| 42 | a[j][k]=dum; |
---|
| 43 | } |
---|
| 44 | *d = -(*d); |
---|
| 45 | vv[imax]=vv[j]; |
---|
| 46 | } |
---|
| 47 | //indx[j]=imax; |
---|
| 48 | if (a[j][j] == 0.0) a[j][j]=1.0e-20; |
---|
| 49 | if (j != n) { |
---|
| 50 | dum=1.0/(a[j][j]); |
---|
| 51 | for (i=j+1;i<=n;i++) a[i][j] *= dum; |
---|
| 52 | } |
---|
| 53 | } |
---|
| 54 | |
---|
| 55 | } |
---|
| 56 | |
---|
| 57 | |
---|
| 58 | /* D = detn(X) vectorized determinant; D(i1,...,in) = det(X(:,:,i1,...,in)). */ |
---|
| 59 | void mexFunction( int nargout, |
---|
| 60 | mxArray **argout, |
---|
| 61 | int nargin, |
---|
| 62 | const mxArray **argin |
---|
| 63 | ) |
---|
| 64 | { |
---|
| 65 | int ndims, n, i, j, k, N; |
---|
| 66 | const int *dim; |
---|
| 67 | double *X, *x, *D, **a, *vv, *d; |
---|
| 68 | |
---|
| 69 | if ( nargin != 1 ) mexErrMsgTxt("One input parameter required."); |
---|
| 70 | |
---|
| 71 | if ( mxGetClassID(argin[0]) != mxDOUBLE_CLASS ) mexErrMsgTxt("X must be double."); |
---|
| 72 | ndims = mxGetNumberOfDimensions(argin[0]); |
---|
| 73 | if ( ndims < 2 ) mexErrMsgTxt("X must have 2+ dimensions."); |
---|
| 74 | dim = mxGetDimensions(argin[0]); |
---|
| 75 | if ( dim[0]!=dim[1] ) mexErrMsgTxt("X must have first two dimensions equal."); |
---|
| 76 | X = (double*)mxGetPr(argin[0]); |
---|
| 77 | |
---|
| 78 | if ( ndims>2 ) |
---|
| 79 | argout[0] = mxCreateNumericArray(ndims-2,dim+2,mxDOUBLE_CLASS,mxREAL); |
---|
| 80 | else |
---|
| 81 | argout[0] = mxCreateDoubleMatrix(1,1,mxREAL); |
---|
| 82 | if ( argout[0]==0 ) mexErrMsgTxt("Out of memory."); |
---|
| 83 | D = (double*)mxGetPr(argout[0]); |
---|
| 84 | |
---|
| 85 | // Allocate aux. array a |
---|
| 86 | a = (double**)malloc((dim[0]+1)*sizeof(double*)); |
---|
| 87 | for ( i = 0; i <= dim[0]; i++ ) a[i] = (double*)malloc((dim[1]+1)*sizeof(double)); |
---|
| 88 | |
---|
| 89 | vv=(double*)malloc((dim[0]+1)*sizeof(double)); |
---|
| 90 | |
---|
| 91 | N = 1; for ( i = 2; i < ndims; i++ ) N *= dim[i]; // N = number of determinants |
---|
| 92 | for ( n = 0, x = X, d = D; n < N; n++, x+=dim[0]*dim[1], d++ ) { |
---|
| 93 | if ( dim[0]==2 ) |
---|
| 94 | *d = x[0]*x[3]-x[2]*x[1]; |
---|
| 95 | else if ( dim[0]==3 ) |
---|
| 96 | *d = x[0]*x[4]*x[8]+x[2]*x[3]*x[7]+x[1]*x[5]*x[6]-x[2]*x[4]*x[6]-x[1]*x[3]*x[8]-x[0]*x[5]*x[7]; |
---|
| 97 | else { |
---|
| 98 | k = 0; for ( i = 1; i <= dim[0]; i++) for ( j = 1; j <= dim[1]; j++ ) a[j][i] = x[k++]; |
---|
| 99 | ludcmp(a,dim[0],d,vv); |
---|
| 100 | for ( i=1;i<=dim[0];i++) *d *= a[i][i]; |
---|
| 101 | } |
---|
| 102 | } |
---|
| 103 | |
---|
| 104 | free(vv); |
---|
| 105 | for ( i = 0; i < dim[0]; i++ ) free(a[i]); |
---|
| 106 | free(a); |
---|
| 107 | } |
---|