/* Nearest triangle search * * T = TSRCHMX(X,Y,TRI,XI,YI) returns an index into the rows of TRI * or each point in XI,YI. TSEARCH returns NaN for all points * outside the convex hull. Requires a triangularization TRI of the * points X,Y obtained from DELAUNAY. * * Change History (most recent first): * * <3> 10/13/98 ZPY Do a closest point search followed by * a closest side search to reduce memory. * <2+> 8/3/95 CMT port to UNIX * <2> 8/3/95 CMT Search down nearest triangle list (which * must be passed down). Do an exact distance * check as final part of search. * <1> 7/30/95 CMT Initial revision. * * Zhiping You, 10/13/98, Clay M. Thompson 8-15-95 * Copyright (c) 1984-98 by The MathWorks, Inc. * $Revision: 1.5 $ */ #include "mex.h" #include #define DOUBLE2INT(x) ((int) x) #define malloc(siz) mxCalloc(1,siz) #define free(x) mxFree(x) #define HandleError mexErrMsgTxt #define sign(x) (x > 0.0 ? 1 : (x < 0.0 ? -1 : 0)) /* Distance from (vx,vy) to (x(k), y(k)) */ #define dist(vx,vy,k) ((x[k]-vx)*(x[k]-vx) + (y[k]-vy)*(y[k]-vy)) /* Global variables */ double *x, *xi; double *y, *yi; double *tri; int nxy, ntri; int *ir,*jc; /* Nearest Neighbor sparse structure */ bool warned; /* True if point (vx,vy) is inside triangle t */ int inside(double vx,double vy,int t) { int s1, s2, s3; double x1,x2,x3,y1,y2,y3; int k1,k2,k3; k1 = DOUBLE2INT(tri[t])-1; k2 = DOUBLE2INT(tri[t+ntri])-1; k3 = DOUBLE2INT(tri[t+2*ntri])-1; x1 = x[k1]-vx; x2 = x[k2]-vx; x3 = x[k3]-vx; y1 = y[k1]-vy; y2 = y[k2]-vy; y3 = y[k3]-vy; /* * A point is inside the triangle if all cross products of the * vectors from this point to the vertices are all positive or * all negative. */ s1 = sign(x1*y2 - x2*y1); s2 = sign(x2*y3 - x3*y2); if (s1 * s2 < 0) return(0); s3 = sign(x3*y1 - x1*y3); if (s2 * s3 < 0) return(0); if (s1 * s3 < 0) return(0); return(1); } /* Search for nearest point to k starting from p */ static int search(int k,int p) { double vx, vy; int prev,i, j, m, n, flag, count; double d,mindist; vx = xi[k]; vy = yi[k]; m = 3*ntri; count=3*ntri; mindist = dist(vx,vy,p); do { prev = p; for (flag = 0, j = jc[p]; j < jc[p+1] && count > 0; j++, count--) { for (i = ir[j]; i < m; i += ntri) { /* go through three vertices of the triangle ir[j]*/ n = (int)tri[i]-1; if (n == p) continue; d = dist(vx,vy,n); if (d < mindist) { mindist = d; p = n; flag = 1; break; } } if (flag) break; } if (count == 0) { if (!warned) { mexWarnMsgTxt("One or more points did not converge."); warned = true; } break; } } while (prev != p); return(p); } /* distance from (vx,vy) to edge i to k */ double sidedist(double vx,double vy, int i, int j) { double x1, x2, y1, y2; double s, dx, dy; x1 = x[i]; x2 = x[j]; y1 = y[i]; y2 = y[j]; s = (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1); if (s != 0) { s = ((x2-x1)*(vx-x1)+(y2-y1)*(vy-y1)) / s; s = (s < 0.0 ? 0.0 : ((s > 1.0) ? 1.0 : s)); } dx = x1 + s*(x2-x1) - vx; dy = y1 + s*(y2-y1) - vy; return(dx*dx + dy*dy); } /* Search for nearest side to the point k starting from point p, which is the closest vertex returned by search(). The two vertices of the returned side will be stored in s. (i.e. s[0] and s[1]) */ int sidesearch(int k, int p) { double vx, vy; int prev, minp, i, j, m, n, flag, count; double d, mindist; vx = xi[k]; vy = yi[k]; m = 3*ntri; count=3*ntri; mindist = dist(vx,vy,p); minp = p; do { prev = p; for (flag=0, j = jc[p]; (j < jc[p+1]) && (count > 0); j++, count--) { for (i = ir[j]; i < m; i += ntri) { n = (int)tri[i]-1; if (n == p) continue; d = sidedist(vx,vy,p,n); if (d < mindist) { mindist = d; minp = n; } } } p = minp; if (count == 0) { if (!warned) { mexWarnMsgTxt("One or more points did not converge."); warned = true; } break; } } while (prev != p); return(p); } void mexFunction(int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[]) { int i, j, k, m, n, t; double *p,*q; mxArray *ins[5]; mxArray *p2t[1]; double nan; double s; if (nrhs > 5) mexErrMsgTxt("Too many input arguments."); if (nrhs < 5) mexErrMsgTxt("Not enough input arguments."); if (nlhs > 1) mexErrMsgTxt("Too many output arguments."); if (mxGetNumberOfElements(prhs[0]) != mxGetNumberOfElements(prhs[1])) mexErrMsgTxt("X and Y must be the same length."); if ((mxGetM(prhs[3]) != mxGetM(prhs[4])) || (mxGetN(prhs[3]) != mxGetN(prhs[4]))) mexErrMsgTxt("XI and YI must the same size."); if (mxGetN(prhs[2])!=3) HandleError("TRI must be M-by-3."); nxy = mxGetNumberOfElements(prhs[0]); x = mxGetPr(prhs[0]); y = mxGetPr(prhs[1]); ntri = mxGetM(prhs[2]); tri = mxGetPr(prhs[2]); n = mxGetNumberOfElements(prhs[3]); xi = mxGetPr(prhs[3]); yi = mxGetPr(prhs[4]); /* if no triangle is given, return []. */ if (ntri == 0) { plhs[0] = mxCreateDoubleMatrix(0,0,mxREAL); return; } plhs[0] = mxCreateDoubleMatrix(mxGetM(prhs[3]),mxGetN(prhs[3]),mxREAL); nan = mxGetNaN(); /* repmat((1:ntri)',1,3) */ ins[0] = mxCreateDoubleMatrix(ntri,3,mxREAL); p = mxGetPr(ins[0]); for (j = 0; j < 3; j++) for (i = 1; i <= ntri; i++) *p++ = i; /* tri */ ins[1] = mxCreateDoubleMatrix(ntri,3,mxREAL); p = mxGetPr(ins[1]); for (i=0, q = ((double *)mxGetPr(prhs[2])); i < 3*ntri; i++) { *p = *q++; if ((*p < 0) || (*p > nxy) || (*p != floor(*p))) mexErrMsgTxt("TRI array contains values outside the range of X and Y."); p++; } /* 1 */ ins[2] = mxCreateDoubleMatrix(1,1,mxREAL); ((double *)mxGetPr(ins[2]))[0] = 1.0; /* ntri */ ins[3] = mxCreateDoubleMatrix(1,1,mxREAL); ((double *)mxGetPr(ins[3]))[0] = ntri; /* nxy */ ins[4] = mxCreateDoubleMatrix(1,1,mxREAL); ((double *)mxGetPr(ins[4]))[0] = nxy; /* sparse(repmat((1:ntri)',1,3),tri,1,ntri,nxy). */ p2t[0] = 0L; mexCallMATLAB(1,p2t,5,ins,"sparse"); /* Free inputs */ mxDestroyArray(ins[0]); mxDestroyArray(ins[1]); mxDestroyArray(ins[2]); mxDestroyArray(ins[3]); mxDestroyArray(ins[4]); if (!mxIsSparse(p2t[0])) mexErrMsgTxt("point to triangle matrix must be sparse."); ir = mxGetIr(p2t[0]); /* indexes of points */ jc = mxGetJc(p2t[0]); /* indexes of points */ warned = false; /* If possible, start at the first point in the first triangle */ if (ntri > 0) k = ((int) tri[0]) - 1; for (t = 0, i = 0, p = mxGetPr(plhs[0]); i < n; ++i) { if (inside(xi[i],yi[i],t)) { *p++ = t+1; continue; } k = search(i,k); /* search the closest point */ /* search for a point that is part of the closest side */ m = sidesearch(i, k); for (j = jc[m]; j < jc[m+1]; j++) { /* find the triangle that contains m */ t = ir[j]; if (inside(xi[i], yi[i], t)) { *p++ = t+1; break; } } if (j == jc[m+1]) *p++ = nan; /* not inside any triangle */ } mxDestroyArray(p2t[0]); }