void mexFunction(int nlhs, mxArray * plhs[], int nrhs, const mxArray * prhs[]){ // check number of arguments if( nrhs!=2 ) mexErrMsgTxt("This function requires 2 arguments\n"); if( !mxIsNumeric(prhs[0]) ) mexErrMsgTxt("varargin{0} must be a valid kdtree pointer\n"); if( !mxIsNumeric(prhs[1]) ) mexErrMsgTxt("varargin{1} must be a query set of points\n"); // retrieve the tree pointer KDTree* tree; retrieve_tree( prhs[0], tree ); // retrieve the query data double* query_data; int npoints, ndims; retrieve_data( prhs[1], query_data, npoints, ndims ); // printf("query size: %dx%d\n", npoints, ndims); // check dimensions if( ndims != tree->ndims() ) mexErrMsgTxt("vararg{1} must be a [Nxk] matrix of N points in k dimensions\n"); // npoints x 1 indexes in output plhs[0] = mxCreateDoubleMatrix(npoints, 1, mxREAL); double* indexes = mxGetPr(plhs[0]); // cout << "nindexes: " << mxGetM(plhs[0]) << "x" << mxGetN(plhs[0]) << endl; // execute the query FOR EVERY point storing the index vector< double > query(ndims,0); for(int i=0; i<npoints; i++) for( int j=0; j<ndims; j++ ){ query[j] = query_data[ i+j*npoints ]; indexes[i] = tree->closest_point(query)+1; } }
void mexFunction(int nlhs, mxArray * plhs[], int nrhs, const mxArray * prhs[]) { if(mxIsClass(prhs[0],"kdtree")==0) { mexPrintf("First argument must be a kdtree class\n"); return; } KDTree *tree = KDTree::unserialize(mxGetPr(mxGetFieldByNumber(prhs[0],0,0))); // Verify the point array if (mxGetNumberOfDimensions(prhs[1]) != 2) { mexPrintf("Invalid point array passed in.\n"); return; } int npoints = (int) mxGetM(prhs[1]); int ndim = (int) mxGetN(prhs[1]); if (ndim != tree->ndim) { mexPrintf("Points have wrong number of dimensions.\n"); mexPrintf("Tree dimension = %d\n",tree->ndim); mexPrintf("Input array dimension = %d\n",ndim); delete tree; return; } // Check the format of the input bool isDouble = false; mxClassID id = mxGetClassID(prhs[1]); double *dPtr = (double *)0; float *sPtr = (float *)0; if (id == mxDOUBLE_CLASS) { dPtr = (double *) mxGetPr(prhs[1]); isDouble = true; } else if (id == mxSINGLE_CLASS) { sPtr = (float *) mxGetPr(prhs[1]); isDouble = false; } else { mexPrintf("Input points must be either sinlgle or double\n"); delete tree; return; } // Create an output array of indices plhs[0] = mxCreateNumericMatrix(npoints, 1, mxDOUBLE_CLASS, mxREAL); double *idxptr = (double *) mxGetPr(plhs[0]); // Create an output array of points, if needed float *fpntptr = (float *) 0; double *dpntptr = (double *) 0; if (nlhs > 1) { if (isDouble) { plhs[1] = mxCreateNumericMatrix(npoints, ndim, mxDOUBLE_CLASS, mxREAL); dpntptr = (double *) mxGetPr(plhs[1]); } else { plhs[1] = mxCreateNumericMatrix(npoints, ndim, mxSINGLE_CLASS, mxREAL); fpntptr = (float *) mxGetPr(plhs[1]); } } // Allocate the point to check float *curPoint = new float[ndim]; for (int i = 0; i < npoints; i++) { // Extract the point in the correct format if (isDouble) { for (int j = 0; j < ndim; j++) { // i*ndims+j // j*npoints + i; // MATLAB stores the transpose of the normal order curPoint[j] = (float) dPtr[j * npoints + i]; } } else { for (int j = 0; j < ndim; j++) curPoint[j] = sPtr[j * npoints + i]; } // Check the point int idx; tree->closest_point(curPoint, idx); // Set the output arrays // First the index // adding 1 since MATLAB arrays are referenced to // 1, not zero idxptr[i] = (double) (idx + 1); // Then, the actual point -- if requested if (nlhs > 1) { if (isDouble) { for (int j = 0; j < ndim; j++) dpntptr[j * npoints + i] = tree->points[idx*tree->ndim+j]; } else { for (int j = 0; j < ndim; j++) fpntptr[j * npoints + i] = tree->points[idx*tree->ndim+j]; } } } // Deallocate the point to check delete curPoint; // Get rid of the tree // (This doesn't delete the serialized data) delete tree; return; } // end of kdtree_closestpoint