void Gridder::find(at::real x, at::real y, at::real range, SegmentArray& results) const { results.clear(); int ix0 = (int) ((x - range - x0)/metersPerCell); int iy0 = (int) ((y - range - y0)/metersPerCell); int ix1 = (int) ((x + range - x0)/metersPerCell); int iy1 = (int) ((y + range - y0)/metersPerCell); for (int iy=iy0; iy<=iy1; ++iy) { for (int ix=ix0; ix<=ix1; ++ix) { if (ix >=0 && iy >=0 && ix < width && iy < height) { for (Segment* s = cells[sub2ind(ix,iy)]; s; s = s->nextGrid) { results.push_back(s); } } } } }
MatrixXd CGppe::deriv2_log_likelihood_CGppe_fast(double sigma, TypePair all_pairs, VectorXd idx_global_1, VectorXd idx_global_2, int M, int N) { VectorXd deriv_loglike, z, cdf_val, pdf_val, val, ratio, all_diag_idx, ind, ind_trans; M = all_pairs.rows(); int n = M * N; VectorXd consec(n); MatrixXd Deriv2 = MatrixXd::Zero(n, n); for (int i = 0;i < n;i++) { consec(i) = i; } all_diag_idx = sub2ind(n, n, consec, consec); z = (GetVec(f, idx_global_1) - GetVec(f, idx_global_2)) / sigma; cdf_val = normcdf(z); pdf_val = normpdf(z); ratio = pdf_val.array() / cdf_val.array(); val = -(1. / pow(sigma, 2)) * (ratio.array() * (z + ratio).array()); ind = sub2ind(n, n, idx_global_1, idx_global_2); Deriv2 = SetMatGenIdx(Deriv2, ind, -val); ind_trans = sub2ind(n, n, idx_global_2, idx_global_1); Deriv2 = SetMatGenIdx(Deriv2, ind_trans, -val); Deriv2 = SetMatGenIdx(Deriv2, all_diag_idx, GetMatGenIdx(Deriv2, all_diag_idx) + Get_Cumulative_Val(idx_global_1, val, n)); Deriv2 = SetMatGenIdx(Deriv2, all_diag_idx, GetMatGenIdx(Deriv2, all_diag_idx) + Get_Cumulative_Val(idx_global_2, val, n)); return Deriv2; }
void Gridder::add(at::real x, at::real y, Segment* s) { int ix = (int) ((x - x0)/metersPerCell); int iy = (int) ((y - y0)/metersPerCell); if (ix >=0 && iy >=0 && ix < width && iy < height) { size_t idx = sub2ind(ix,iy); s->nextGrid = cells[idx]; cells[idx] = s; } }
int get_position(char *pos) { int a = (int)pos[0]; int b = (int)pos[1]; a = a - 97; b = b - 49; float coordinate = b * 4 + a; int ret = sub2ind(a, b); return ret; }
/* construct the nd dimensional histogram */ void histcND( double* h, double* A, double* wtMask, int n, int nd, double**edges, int* nBins ) { int i, j, k, inbounds; int *subMul, *sub, ind; sub = (int *) mxMalloc( nd * sizeof(int) ); subMul = sub2ind_init( nBins, nd ); for( i=0; i < n; i++) { inbounds = 1; for( j=0; j < nd; j++) { sub[j] = findBin( A[ n*j+i ], edges[j], nBins[j] ); if(sub[j]==nBins[j]) { inbounds=0; break; } } if( inbounds ) { sub2ind(ind, sub, subMul, nd); h[ ind ] += wtMask[i]; } } mxFree( sub ); mxFree( subMul ); }
constexpr auto _mem_offset_at_seq(const tensor_aligned_data_base<ET, ShapeT, T> &t, const const_ints<size_t, Is...> &, const SubTs &... subs) { return sub2ind(t.shape(), subs * t.sub_scale(const_index<Is>()) + t.sub_offset(const_index<Is>())...); }
inline decltype(auto) element_at(tensor_continuous_data_base<ET, ShapeT, T> &t, const SubTs &... subs) { assert(subscripts_are_valid(t.shape(), subs...)); return t.ptr()[sub2ind(t.shape(), subs...)]; }
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { // If mex file is unloaded, do the shutdown (to free the bluetooth and release the memory) mexAtExit(shutdown); //Check the number of received parameters to the Mex function if(nrhs < 1) { mexErrMsgTxt("There must be at least one argument. Run help blueTeraMex for usage\n"); return; } int cmd = (unsigned int) mxGetScalar(prhs[0]); mxArray *arrayOutput = NULL; mxArray *singleoutput = NULL; double *outArray; switch(cmd) { // Initialize the system case 0: initialize(); break; // Connect to the sensors(s) case 1: if(nrhs < 2) { mexErrMsgTxt("The second argument must be a cell array with the hardware addresses of the sensors to connect to. Run help blueTeraMex for usage\n"); return; } connectToSensors(prhs[1]); break; // Return number of sensors and their states case 2: singleoutput = mxCreateDoubleMatrix(1,1, mxREAL); plhs[0] = singleoutput; *(mxGetPr(plhs[0])) = (double) numSensors; arrayOutput = mxCreateDoubleMatrix(1,numSensors,mxREAL); plhs[1] = arrayOutput; outArray = mxGetPr(plhs[1]); for (mwSize i=0;i<numSensors;i++) { outArray[i] = state[i]; } break; // Return the data case 3: // cols = 4 quaternions + 3 acceleration + 2 * time stamps arrayOutput = mxCreateDoubleMatrix(numSensors,9,mxREAL); plhs[0] = arrayOutput; outArray = mxGetPr(plhs[0]); for (mwSize i=0;i<numSensors;i++) { // Matlab arrays are 1D, so need to find the right index outArray[sub2ind(i,0,numSensors)] = quaternions[i][0]; outArray[sub2ind(i,1,numSensors)] = quaternions[i][1]; outArray[sub2ind(i,2,numSensors)] = quaternions[i][2]; outArray[sub2ind(i,3,numSensors)] = quaternions[i][3]; outArray[sub2ind(i,4,numSensors)] = accelerations[i][0]; outArray[sub2ind(i,5,numSensors)] = accelerations[i][1]; outArray[sub2ind(i,6,numSensors)] = accelerations[i][2]; outArray[sub2ind(i,7,numSensors)] = quaternionUpdateTime[i]; outArray[sub2ind(i,8,numSensors)] = accelerationUpdateTime[i]; } break; case 4: shutdown(); break; // Scan for devices case 5: {int scantime = 10; if(nrhs >= 2) { scantime = (int)*mxGetPr(prhs[1]); } IOTE_StartScan(true); // only scan for blueTera devices mexPrintf("Scanning, please wait %d seconds. . .\n",scantime); // Needed to print the string now mexEvalString("drawnow;"); Sleep(scantime*1000); IOTE_StopScan(); mexPrintf("Discovered %d sensor(s)\n",discovered); for(int k=0;k<discovered;k++) { mexPrintf("Sensor %d: %s\n",k,discoveredAddresses[k]); } // Return the values in a cell array mxArray *cell_array = mxCreateCellMatrix((mwSize)discovered,1); for(int k=0; k<discovered; k++){ mxSetCell(cell_array,k,mxCreateString(discoveredAddresses[k])); } plhs[0] = cell_array; break; } } }