Line | |
---|
1 | #include <math.h> |
---|
2 | #include "mex.h" |
---|
3 | #include "matrix.h" |
---|
4 | |
---|
5 | #define f1 (*F) |
---|
6 | #define f2 (*(F+1)) |
---|
7 | #define f3 (*(F+2)) |
---|
8 | #define f4 (*(F+3)) |
---|
9 | #define f5 (*(F+4)) |
---|
10 | #define f6 (*(F+5)) |
---|
11 | #define f7 (*(F+6)) |
---|
12 | #define f8 (*(F+7)) |
---|
13 | #define f9 (*(F+8)) |
---|
14 | |
---|
15 | #define u1 (*(u)) |
---|
16 | #define u2 (*(u+1)) |
---|
17 | #define u4 (*(u+3)) |
---|
18 | #define u5 (*(u+4)) |
---|
19 | |
---|
20 | void mexFunction( int dstN, mxArray **aDstP, int aSrcN, const mxArray **aSrcP ) |
---|
21 | { |
---|
22 | double *F = mxGetPr(aSrcP[0]); |
---|
23 | double *u = mxGetPr(aSrcP[1]); |
---|
24 | double *p; |
---|
25 | double rx, ry, rwc, ryc, rxc, r; |
---|
26 | int len = mxGetN(aSrcP[1]); |
---|
27 | int i; |
---|
28 | |
---|
29 | aDstP[0] = mxCreateDoubleMatrix(1, len, mxREAL); |
---|
30 | p = (double *)mxGetData(aDstP[0]); |
---|
31 | |
---|
32 | for (i=1; i<=len; i++) |
---|
33 | { |
---|
34 | rxc = f1 * u4 + f4 * u5 + f7; |
---|
35 | ryc = f2 * u4 + f5 * u5 + f8; |
---|
36 | rwc = f3 * u4 + f6 * u5 + f9; |
---|
37 | r =(u1 * rxc + u2 * ryc + rwc); |
---|
38 | rx = f1 * u1 + f2 * u2 + f3; |
---|
39 | ry = f4 * u1 + f5 * u2 + f6; |
---|
40 | |
---|
41 | *p = r*r / (rxc*rxc + ryc*ryc + rx*rx + ry*ry); |
---|
42 | p ++; |
---|
43 | u += 6; |
---|
44 | } |
---|
45 | } |
---|
Note: See
TracBrowser
for help on using the repository browser.