Rev | Line | |
---|
[37] | 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.