THESE/codes/snake/basic_code/PatchNormalsDouble.c

   1 #include "mex.h"\r
   2 #include "math.h"\r
   3 \r
   4 /* \r
   5  * \r
   6  * function [Nx,Ny,Nz]=patchnormals_double(Fa,Fb,Fc,Vx,Vy,Vz)\r
   7  *\r
   8 */\r
   9 \r
  10 /* Coordinates to index */\r
  11 int mindex3(int x, int y, int z, int sizx, int sizy) { return z*sizx*sizy+y*sizx+x;}\r
  12 int mindex2(int x, int y, int sizx) { return y*sizx+x;}\r
  13 \r
  14 /* The matlab mex function */\r
  15 void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )\r
  16 {\r
  17     /* All inputs */\r
  18     double *FacesA, *FacesB, *FacesC, *VerticesX, *VerticesY, *VerticesZ;\r
  19     \r
  20     /* All outputs, Vertex Normals */\r
  21     double *NormalsX, *NormalsY, *NormalsZ;\r
  22         \r
  23     /* Temporary Face Normals */\r
  24     double *FaceNormalsX, *FaceNormalsY, *FaceNormalsZ;\r
  25     \r
  26     /* Temporary Face angles */\r
  27     double *AnglesA,  *AnglesB,  *AnglesC;\r
  28     \r
  29     /* Number of faces */\r
  30     const mwSize *FacesDims;\r
  31     int FacesN=0;\r
  32    \r
  33     /* Number of vertices */\r
  34     const mwSize *VertexDims;\r
  35     int VertexN=0;\r
  36     int VertexNA[1]={0};    \r
  37     \r
  38     /* 1D Index  */\r
  39     int index0, index1, index2;\r
  40             \r
  41     /* Edge coordinates and lenght */\r
  42     double e0x, e0y, e0z, e0l;\r
  43     double e1x, e1y, e1z, e1l;\r
  44     double e2x, e2y, e2z, e2l;\r
  45           \r
  46     /* Length of normal */\r
  47     double nl;\r
  48     \r
  49     /* Loop variable */\r
  50     int i;\r
  51             \r
  52     /* Check for proper number of arguments. */\r
  53    if(nrhs!=6) {\r
  54      mexErrMsgTxt("6 inputs are required.");\r
  55    } else if(nlhs!=3) {\r
  56      mexErrMsgTxt("3 outputs are required");\r
  57    }\r
  58    \r
  59    /* Read all inputs (faces and vertices) */\r
  60    FacesA=mxGetPr(prhs[0]);\r
  61    FacesB=mxGetPr(prhs[1]);\r
  62    FacesC=mxGetPr(prhs[2]);\r
  63    VerticesX=mxGetPr(prhs[3]);\r
  64    VerticesY=mxGetPr(prhs[4]);\r
  65    VerticesZ=mxGetPr(prhs[5]);\r
  66 \r
  67    /* Get number of FacesN */\r
  68    FacesDims = mxGetDimensions(prhs[0]);   \r
  69    FacesN=FacesDims[0]*FacesDims[1];\r
  70    \r
  71    /* Get number of VertexN */\r
  72    VertexDims = mxGetDimensions(prhs[3]);   \r
  73    VertexN=VertexDims[0]*VertexDims[1];\r
  74    \r
  75    /* Create Output arrays for the Normal coordinates */\r
  76    VertexNA[0]=VertexN;\r
  77    plhs[0] = mxCreateNumericArray(1, VertexNA, mxDOUBLE_CLASS, mxREAL);\r
  78    plhs[1] = mxCreateNumericArray(1, VertexNA, mxDOUBLE_CLASS, mxREAL);\r
  79    plhs[2] = mxCreateNumericArray(1, VertexNA, mxDOUBLE_CLASS, mxREAL);\r
  80    NormalsX = mxGetPr(plhs[0]);\r
  81    NormalsY = mxGetPr(plhs[1]);\r
  82    NormalsZ = mxGetPr(plhs[2]);\r
  83       \r
  84 \r
  85    FaceNormalsX = (double *)malloc( FacesN* sizeof(double) );\r
  86    FaceNormalsY = (double *)malloc( FacesN* sizeof(double) );\r
  87    FaceNormalsZ = (double *)malloc( FacesN* sizeof(double) );\r
  88    \r
  89    AnglesA = (double *)malloc( FacesN* sizeof(double) );\r
  90    AnglesB = (double *)malloc( FacesN* sizeof(double) );\r
  91    AnglesC = (double *)malloc( FacesN* sizeof(double) );\r
  92    \r
  93    \r
  94    /* Calculate all face normals and angles */\r
  95    for (i=0; i<FacesN; i++)\r
  96    {\r
  97        /* Get indices of face vertices */\r
  98        index0=(int)FacesA[i]-1;\r
  99        index1=(int)FacesB[i]-1;\r
 100        index2=(int)FacesC[i]-1;\r
 101       \r
 102        /* Make edge vectors */\r
 103        e0x=VerticesX[index0]-VerticesX[index1];  \r
 104        e0y=VerticesY[index0]-VerticesY[index1];  \r
 105        e0z=VerticesZ[index0]-VerticesZ[index1];\r
 106        \r
 107        e1x=VerticesX[index1]-VerticesX[index2];  \r
 108        e1y=VerticesY[index1]-VerticesY[index2];  \r
 109        e1z=VerticesZ[index1]-VerticesZ[index2];\r
 110        \r
 111        e2x=VerticesX[index2]-VerticesX[index0];  \r
 112        e2y=VerticesY[index2]-VerticesY[index0];  \r
 113        e2z=VerticesZ[index2]-VerticesZ[index0];\r
 114 \r
 115        /* Normalize the edge vectors */\r
 116        e0l = sqrt(e0x*e0x+e0y*e0y+e0z*e0z)+1e-14;\r
 117        e0x/=e0l; e0y/=e0l; e0z/=e0l;\r
 118        e1l = sqrt(e1x*e1x+e1y*e1y+e1z*e1z)+1e-14; \r
 119        e1x/=e1l; e1y/=e1l; e1z/=e1l;\r
 120        e2l = sqrt(e2x*e2x+e2y*e2y+e2z*e2z)+1e-14; \r
 121        e2x/=e2l; e2y/=e2l; e2z/=e2l;\r
 122 \r
 123        /* Calculate angles of face seen from vertices */\r
 124        AnglesA[i]= acos(e0x*(-e2x)+e0y*(-e2y)+e0z*(-e2z));\r
 125        AnglesB[i]= acos(e1x*(-e0x)+e1y*(-e0y)+e1z*(-e0z));\r
 126        AnglesC[i]= acos(e2x*(-e1x)+e2y*(-e1y)+e2z*(-e1z));\r
 127               \r
 128        /* Normal of the face */\r
 129        FaceNormalsX[i]=e0y*(e2z) - e0z * (e2y);\r
 130        FaceNormalsY[i]=e0z*(e2x) - e0x * (e2z);\r
 131        FaceNormalsZ[i]=e0x*(e2y) - e0y * (e2x);\r
 132    }\r
 133    \r
 134    /* Calculate all vertex normals and angles */\r
 135    for (i=0; i<FacesN; i++)\r
 136    {\r
 137        index0=(int)FacesA[i]-1;\r
 138        NormalsX[index0]+= FaceNormalsX[i]*AnglesA[i];\r
 139        NormalsY[index0]+= FaceNormalsY[i]*AnglesA[i];\r
 140        NormalsZ[index0]+= FaceNormalsZ[i]*AnglesA[i];\r
 141        index1=(int)FacesB[i]-1;\r
 142        NormalsX[index1]+= FaceNormalsX[i]*AnglesB[i];\r
 143        NormalsY[index1]+= FaceNormalsY[i]*AnglesB[i];\r
 144        NormalsZ[index1]+= FaceNormalsZ[i]*AnglesB[i];\r
 145        index2=(int)FacesC[i]-1;\r
 146        NormalsX[index2]+= FaceNormalsX[i]*AnglesC[i];\r
 147        NormalsY[index2]+= FaceNormalsY[i]*AnglesC[i];\r
 148        NormalsZ[index2]+= FaceNormalsZ[i]*AnglesC[i];\r
 149    }\r
 150     \r
 151    /* Normalize the Normals */\r
 152    for (i=0; i<VertexN; i++)\r
 153    {\r
 154        nl= sqrt(NormalsX[i]*NormalsX[i]+NormalsY[i]*NormalsY[i]+NormalsZ[i]*NormalsZ[i])+1e-14; ;\r
 155        NormalsX[i]/=nl; NormalsY[i]/=nl; NormalsZ[i]/=nl;\r
 156    }\r
 157 \r
 158    /* Free memory */\r
 159    free(FaceNormalsX);\r
 160    free(FaceNormalsY);\r
 161    free(FaceNormalsZ);\r
 162    free(AnglesA);\r
 163    free(AnglesB);\r
 164    free(AnglesC);\r
 165 }\r
 166  \r
 167 \r
 168 \r