Exemple #1
0
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);
        }

      }
        
    }
  }

}
Exemple #2
0
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;
}
Exemple #3
0
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;
  }
  
}
Exemple #4
0
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;
}
Exemple #5
0
/* 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 );
}
Exemple #6
0
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>())...);
}
Exemple #7
0
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;
        }
    }
    
}