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 | } |
---|