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;
}
}
}
