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
