1 | #include <math.h> |
---|
2 | #include "mex.h" |
---|
3 | #include "matrix.h" |
---|
4 | |
---|
5 | #define a11 (*A) |
---|
6 | #define a12 (*(A+1)) |
---|
7 | #define a13 (*(A+2)) |
---|
8 | #define a21 (*(A+3)) |
---|
9 | #define a22 (*(A+4)) |
---|
10 | #define a23 (*(A+5)) |
---|
11 | #define a31 (*(A+6)) |
---|
12 | #define a32 (*(A+7)) |
---|
13 | #define a33 (*(A+8)) |
---|
14 | |
---|
15 | #define b11 (*B) |
---|
16 | #define b12 (*(B+1)) |
---|
17 | #define b13 (*(B+2)) |
---|
18 | #define b21 (*(B+3)) |
---|
19 | #define b22 (*(B+4)) |
---|
20 | #define b23 (*(B+5)) |
---|
21 | #define b31 (*(B+6)) |
---|
22 | #define b32 (*(B+7)) |
---|
23 | #define b33 (*(B+8)) |
---|
24 | |
---|
25 | |
---|
26 | void mexFunction( int dstN, mxArray **aDstP, int aSrcN, const mxArray **aSrcP ) |
---|
27 | { |
---|
28 | double *A = mxGetPr(aSrcP[0]), *B = mxGetPr(aSrcP[1]); |
---|
29 | double *p; |
---|
30 | int i; |
---|
31 | |
---|
32 | aDstP[0] = mxCreateDoubleMatrix(4, 1, mxREAL); |
---|
33 | p = mxGetPr(aDstP[0]); |
---|
34 | |
---|
35 | *p = -(b13*b22*b31) + b12*b23*b31 + b13*b21*b32 - |
---|
36 | b11*b23*b32 - b12*b21*b33 + b11*b22*b33; |
---|
37 | |
---|
38 | *(p+1) = -(a33*b12*b21) + a32*b13*b21 + a33*b11*b22 - |
---|
39 | a31*b13*b22 - a32*b11*b23 + a31*b12*b23 + |
---|
40 | a23*b12*b31 - a22*b13*b31 - a13*b22*b31 + |
---|
41 | 3*b13*b22*b31 + a12*b23*b31 - 3*b12*b23*b31 - |
---|
42 | a23*b11*b32 + a21*b13*b32 + a13*b21*b32 - |
---|
43 | 3*b13*b21*b32 - a11*b23*b32 + 3*b11*b23*b32 + |
---|
44 | (a22*b11 - a21*b12 - a12*b21 + 3*b12*b21 + a11*b22 - |
---|
45 | 3*b11*b22)*b33; |
---|
46 | |
---|
47 | *(p+2) = -(a21*a33*b12) + a21*a32*b13 + |
---|
48 | a13*a32*b21 - a12*a33*b21 + 2*a33*b12*b21 - |
---|
49 | 2*a32*b13*b21 - a13*a31*b22 + a11*a33*b22 - |
---|
50 | 2*a33*b11*b22 + 2*a31*b13*b22 + a12*a31*b23 - |
---|
51 | a11*a32*b23 + 2*a32*b11*b23 - 2*a31*b12*b23 + |
---|
52 | 2*a13*b22*b31 - 3*b13*b22*b31 - 2*a12*b23*b31 + |
---|
53 | 3*b12*b23*b31 + a13*a21*b32 - 2*a21*b13*b32 - |
---|
54 | 2*a13*b21*b32 + 3*b13*b21*b32 + 2*a11*b23*b32 - |
---|
55 | 3*b11*b23*b32 + a23* |
---|
56 | (-(a32*b11) + a31*b12 + a12*b31 - 2*b12*b31 - |
---|
57 | a11*b32 + 2*b11*b32) + |
---|
58 | (-(a12*a21) + 2*a21*b12 + 2*a12*b21 - 3*b12*b21 - |
---|
59 | 2*a11*b22 + 3*b11*b22)*b33 + |
---|
60 | a22*(a33*b11 - a31*b13 - a13*b31 + 2*b13*b31 + |
---|
61 | a11*b33 - 2*b11*b33); |
---|
62 | |
---|
63 | for (i=0; i < 9; i++) |
---|
64 | B[i] = A[i] - B[i]; |
---|
65 | |
---|
66 | *(p+3) =-(b13*b22*b31) + b12*b23*b31 + b13*b21*b32 - |
---|
67 | b11*b23*b32 - b12*b21*b33 + b11*b22*b33; |
---|
68 | } |
---|