pdd center(pdd p0, pdd p1, pdd p2) { pdd a = p1-p0; pdd b = p2-p0; double c1=abs2(a)*0.5; double c2=abs2(b)*0.5; double d = a % b; double x = p0.x + (c1 * b.y - c2 * a.y) / d; double y = p0.y + (a.x * c2 - b.x * c1) / d; return pdd(x,y); }
void volume() { float avg = 0; for (int i = 0; i < 20; i++) { avg = avg * 0.9 + float(abs2(analogRead(5)- 512)) * 0.1; } running_vol_avg = running_vol_avg * 0.9 + float(abs2(avg)) * 0.1; current_volume = int(avg); }
void volume() { float avg = 0; for (int i = 0; i < 20; i++) { avg = avg * 0.9 + float(abs2(analogRead(sound_in)- 512)) * 0.1; // avg = avg * 0.9 + float(abs2(random(1024)- 512)) * 0.1; } running_vol_avg = running_vol_avg * 0.95 + float(abs2(avg)) * 0.05; current_volume = int(avg); }
/* Returns non-zero if uva & uvb have converged towards u1a & u1b with * a tolerance less than tol * * MATLAB equivalent: * ( sqrt(norm(uva-u1a,2).^2+norm(uvb-u1b,2).^2) / ... * sqrt(norm(u1a,2).^2+norm(u1b,2).^2) ) < tol */ int is_converged(COMPLEX* uva,COMPLEX* u1a,COMPLEX* uvb,COMPLEX* u1b, REAL tol,int nt) { int jj; REAL num,denom; for(jj = 0, num = 0, denom = 0; jj < nt; jj++) { num += (uva[jj][0]/nt-u1a[jj][0])*(uva[jj][0]/nt-u1a[jj][0]) + (uva[jj][1]/nt-u1a[jj][1])*(uva[jj][1]/nt-u1a[jj][1]) + (uvb[jj][0]/nt-u1b[jj][0])*(uvb[jj][0]/nt-u1b[jj][0]) + (uvb[jj][1]/nt-u1b[jj][1])*(uvb[jj][1]/nt-u1b[jj][1]); denom += abs2(&u1a[jj]) + abs2(&u1b[jj]); } return ( sqrt(num)/sqrt(denom) < tol); }
void fe_wfl(hls::stream< ap_uint<32> > sampleFifo, hls::stream< ap_uint<32> > featureFifo, ap_uint<8> windowSize) { #pragma HLS INTERFACE ap_ctrl_none port=return #pragma HLS INTERFACE ap_fifo port=featureFifo #pragma HLS INTERFACE ap_fifo port=sampleFifo ap_uint<32> data; ap_int<32> wflChannel1 = 0; ap_int<32> wflChannel2 = 0; ap_int<16> sampleChannel1 = 0; ap_int<16> sampleChannel2 = 0; ap_int<16> prevSampleChannel1 = 0; ap_int<16> prevSampleChannel2 = 0; ap_uint<8> cntSamples = 0; // Wait for Samples to arrive in FIFO while( windowSize == 0 ) { } while(1) { wflChannel1 = 0; wflChannel2 = 0; // Count zero-crossing for channel 1 & 2 for(cntSamples = 0; cntSamples < windowSize; cntSamples++) { // Read data from Sample-FIFO // 2 16 bit Samples at one position in 32 bit FIFO => Process 2 channels in parallel data = sampleFifo.read(); sampleChannel1 = data(15, 0); sampleChannel2 = data(31, 16); if (cntSamples > 0) { wflChannel1 += abs2(sampleChannel1 - prevSampleChannel1); wflChannel2 += abs2(sampleChannel2 - prevSampleChannel2); } prevSampleChannel1 = sampleChannel1; prevSampleChannel2 = sampleChannel2; } // Write back features to Feature-FIFO featureFifo.write(wflChannel1); featureFifo.write(wflChannel2); } }
void ExpAgc<T,U>::process() { if (_input.size()>0) { _output.resize(_input.size()); if (!_init) { initialize(); } else { U* Inptr = &_input[0]; U* Outptr = &_output[0]; for (unsigned int i=0; i!=_input.size(); i++) { _currentPower = _alpha*_currentPower+_omega*abs2(*Inptr); if (_minPower < _currentPower && _currentPower < _maxPower) { //our gain is already OK - no AGC applied *Outptr = *Inptr; } else { //apply agc if we are too big or too small float gain = sqrt(_avgPower/(_currentPower+_eps)); *Outptr = *Inptr*gain; } ++Inptr; ++Outptr; } } } }
void ExpAgc<T,U>::initialize() { U* Inptr = &_input[0]; _currentPower=0; for (unsigned int i =0; i< _input.size(); i++) { _currentPower+=abs2(*Inptr); ++Inptr; } //normalize power by number of elements _currentPower = _currentPower/_input.size(); if (_currentPower > _maxPower || _currentPower < _minPower) { float gain = sqrt(_avgPower/(_currentPower+_eps)); U* Outptr = &_output[0]; Inptr = &_input[0]; for (unsigned int i =0; i< _input.size(); i++) { *Outptr = (*Inptr)*gain; ++Inptr; ++Outptr; } } _init = true; }
void calcforce(struct Particle* px, struct Particle py){ struct Force sep = dist(px->position,py.position); double d2 = abs2(sep); d2 = -px->mass*py.mass/d2/sqrt(d2); px->acceleration.x += d2*sep.x; px->acceleration.y += d2*sep.y; px->acceleration.z += d2*sep.z; }
void CalcConservativeVaruables(const particle_t *SPH, system_t *system){ system->energy = 0; system->linear_momentum = vec3<double>(0, 0, 0); system->angular_momentum = vec3<double>(0, 0, 0); #pragma omp parallel for for(int i = 0 ; i < N_SPHP ; ++ i){ system->linear_momentum += SPH[i].m * SPH[i].v; system->angular_momentum += SPH[i].m * outer(SPH[i].r, SPH[i].v); system->energy += SPH[i].m * (0.5 * abs2(SPH[i].v) + SPH[i].u); } }
pair<pdd,double> solve(){ random_shuffle(p,p+n); r2=0; for (int i=0; i<n; i++){ if (abs2(cen-p[i]) <= r2) continue; cen = p[i]; r2 = 0; for (int j=0; j<i; j++){ if (abs2(cen-p[j]) <= r2) continue; cen = 0.5 * (p[i]+p[j]); r2 = abs2(cen-p[j]); for (int k=0; k<j; k++){ if (abs2(cen-p[k]) <= r2) continue; cen = center(p[i],p[j],p[k]); r2 = abs2(cen-p[k]); } } } return {cen,r2}; }
void problem6(){ cout<<"In problem # 6"<<endl<<endl; cout << "Problem 16.4 Gaddis 8th\n\n"; //declare variable Absolut <int>abs1(-5); Absolut <float> abs2(-4.3); //demonstrate templates using two different data types cout << "This program demonstrates Templates using different data types\n"; cout <<"_________________________\n"; cout << "This program returns the absolute value of a number inputed\n\n"; cout << "The absolute value of " << abs1.getA() << " is " << abs1.FndAbs(); cout << endl; cout << "The absolute value of " << abs2.getA() << " is " << abs2.FndAbs(); cout<<endl<<endl; }
int main(){ //srand(42424242); struct Particle* s1; struct Particle* s2; int n = 1000; s1 = randomsystem(n); s2 = randomsystemtree(n); struct Force dacc; double da = 0.; for (int i = 0; i < n; ++i) { // printf("%1.16e %1.16e\n", s1[i].acceleration.x, s2[i].acceleration.x); da += sqrt(dist2(s1[i].acceleration, s2[i].acceleration)/abs2(s1[i].acceleration)); // da += sqrt(abs2(s2[i].acceleration)/abs2(s1[i].acceleration)); } printf("%1.16e\n", da/n); }
int main() { int i ,j, a[10][10]; float k; i; if (i<10) j = abs(i,k); else j = abs2(i); while (i<10) { k = k+abs(1.0); } i = i +abs(a[j], i, 30); j; }
double *dft_ldos::ldos() const { // we try to get the overall scale factor right (at least for a point source) // so that we can compare against the analytical formula for testing // ... in most practical cases, the scale factor won't matter because // the user will compute the relative LDOS of 2 cases (e.g. LDOS/vacuum) // overall scale factor double Jsum_all = sum_to_all(Jsum); double scale = 4.0/pi // from definition of LDOS comparison to power * -0.5 // power = -1/2 Re[E* J] / (Jsum_all * Jsum_all); // normalize to unit-integral current double *sum = new double[Nomega]; for (int i = 0; i < Nomega; ++i) /* 4/pi * work done by unit dipole */ sum[i] = scale * real(Fdft[i] * conj(Jdft[i])) / abs2(Jdft[i]); double *out = new double[Nomega]; sum_to_all(sum, out, Nomega); delete[] sum; return out; }
float exponential(float ld){ float result = 1.0; float term = ld; int diaminator = 2; int count = 0; int test = 0; while(count < 20){ result = result + term; term = term * ld; term = term / (float)diaminator; (int)diaminator ++; count ++; test = (int)(abs2(term)*1000); if(test < 10){ break; } } return result; }
void mainfunction(int *network,int *minNetwork,int size) { //calculate the b w auto dn = genDN(network,size); auto bw = calmaxBW(dn); //76,272 //std::cout<<"B = "<<bw.first<<" W = "<<bw.second<<std::endl; //calculate the supplement node int* supp_node = new int [size]; for(int i = 0; i < size; ++i) supp_node[i] = 0; int *supp_network = subMatrix(network,minNetwork,size,size); for(int i = 0; i < size; ++i){ for(int j = 0; j < size; ++j){ if(supp_network[i*size+j] != 0){ supp_node[j] ++; //outdegree!!! supp_node[i] ++; //indegree!!! } } } //calculate the b d int *node_num = new int[size]; for(int i = 0; i < size; ++i){ node_num[i] = i+1; //set node index } double *bd = calBD(network,size); //pop sort for(int i = 0; i < size; ++i){ for(int j = 0; j < size; ++j){ if(bd[i]>bd[j]){ swap2(bd[i],bd[j]); swap2(node_num[i],node_num[j]); swap2(supp_node[i],supp_node[j]); } } } //print the result //for(int i = 0; i < size; ++i){ //std::cout<<std::setw(2)<<node_num[i]<<" "<<std::setw(10)<<bd[i]<<" "<<supp_node[i]<<std::endl; //} //calculate the correlation double correlation = 0; double bd_total = 0; double indegree_total = 0; double deviation = 0; for(int i = 1; i < size; ++i){ bd_total += bd[i] * bd[i]; indegree_total += supp_node[i] * supp_node[i]; } for(int i = 1; i < size; ++i){ correlation += (abs2(bd[i])/sqrt(bd_total)) * (supp_node[i]/sqrt(indegree_total)); deviation += supp_node[i] * supp_node[i]; } //std::cout<<sqrt(deviation)/indegree_total<<" "<<correlation<<std::endl; std::cout<<bw.first<<" "<<correlation<<std::endl; //else std::cout<<0.1<<" "<<correlation<<std::endl; delete supp_node; delete node_num; delete bd; }
friend complex_op abs(complex_op x) { return sqrt(abs2(x)); }
/* main function */ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { if (nrhs<1) mexErrMsgTxt("ERROR:eikonal3: not enough input elements\n"); if (nrhs>2) mexErrMsgTxt("ERROR:eikonal3: too many input elements.\n"); if (nlhs>2) mexErrMsgTxt("ERROR:eikonal3: too many output elements.\n"); if (mxIsSingle(prhs[0])==0) mexErrMsgTxt("ERROR:eikonal3: first input must be an 3d single matrix\n"); if (nrhs==2 && mxIsDouble(prhs[1])==0) mexErrMsgTxt("ERROR:eikonal3: second input must be an double matrix\n"); if (nrhs==2 && mxGetNumberOfElements(prhs[1])!=3) mexErrMsgTxt("ERROR:eikonal3: second input must have 3 Elements"); int i, j, k, n, ind, x, y, z; int ni, kll=0; float diff, maxdiffi, maxdiff=1, Dio, DNi; float TH=0.01; /* main informations about input data (size, dimensions, ...) */ const mwSize *sL = mxGetDimensions(prhs[0]); const int dL = mxGetNumberOfDimensions(prhs[0]); const int nL = (int)mxGetNumberOfElements(prhs[0]); const int sS[] = {1,3}; mxArray *SS = mxCreateNumericArray(2,sS,mxDOUBLE_CLASS,mxREAL); double *S = mxGetPr(SS); if (nrhs<2) {S[0]=1; S[1]=1; S[2]=1;} else {S = mxGetPr(prhs[1]);} float s1 = abs2((float)S[0]),s2 = abs2((float)S[1]),s3 = abs2((float)S[2]); const float s12 = sqrt( s1*s1 + s2*s2); /* xy - voxel size */ const float s13 = sqrt( s1*s1 + s3*s3); /* xz - voxel size */ const float s23 = sqrt( s2*s2 + s3*s3); /* yz - voxel size */ const float s123 = sqrt(s12*s12 + s3*s3); /* xyz - voxel size */ /* indices of the euclidean distance NW */ const float ND[] = {s123, s12, s123, s13, s1, s13, s123, s12, s123, s23, s2, s23, s3, 0.0, s3, s23, s2, s23, s123, s12, s123, s13, s1, s13, s123, s12, s123}; /* main volumes - actual without memory optimation ... */ plhs[0] = mxCreateNumericArray(dL,sL,mxSINGLE_CLASS,mxREAL); plhs[1] = mxCreateNumericArray(dL,sL,mxSINGLE_CLASS,mxREAL); float *D = (float *)mxGetPr(plhs[0]); float *L = (float *)mxGetPr(plhs[1]); /* input variables */ float *SEG = (float *)mxGetPr(prhs[0]); if ( TH>=0.5 || TH<0.0001 ) mexErrMsgTxt("ERROR:eikonal3: threshhold must be >0.0001 and smaller than 0.5\n"); /* intitialisiation */ for (i=0;i<nL;i++) { if ( SEG[i]<0 ) {D[i]=0; L[i]=SEG[i];} else {D[i]=FLT_MAX; L[i]=0;} } while ( ( maxdiff > TH ) && kll<2000 ) { maxdiffi=0; kll++; for (z=0;z<sL[2];z++) for (y=0;y<sL[1];y++) for (x=0;x<sL[0];x++) { ind = index(x,y,z,sL); if ( SEG[ind]>0 && SEG[ind]<FLT_MAX) { /* read neighbor values */ Dio = D[ind]; n = 0; /* go through all elements in a 3x3x3 box */ for (i=-1;i<=1;i++) for (j=-1;j<=1;j++) for (k=-1;k<=1;k++) { ni = index(x+i,y+j,z+k,sL); if ( SEG[ind]!=FLT_MAX ) { if ( ((x+i)>=0) && ((x+i)<sL[0]) && ((y+j)>=0) && ((y+j)<sL[1]) && ((z+k)>=0) && ((z+k)<sL[2]) ) { DNi = D[ni] + ND[n]*SEG[ind]; if ( DNi<D[ind] ) {D[ind]=DNi; L[ind]=L[ni]; } } } n++; } diff = abs2( Dio - D[ind] ); if ( maxdiffi<diff ) maxdiffi=diff; } } for (z=sL[2]-1;z>=0;z--) for (y=sL[1]-1;y>=0;y--) for (x=sL[0]-1;x>=0;x--) { ind = index(x,y,z,sL); if ( SEG[ind]>0 && SEG[ind]<FLT_MAX) { /* read neighbor values */ Dio = D[ind]; n = 0; /* go through all elements in a 3x3x3 box */ for (i=-1;i<=1;i++) for (j=-1;j<=1;j++) for (k=-1;k<=1;k++) { ni = index(x+i,y+j,z+k,sL); if ( SEG[ind]!=FLT_MAX ) { if ( ((x+i)>=0) && ((x+i)<sL[0]) && ((y+j)>=0) && ((y+j)<sL[1]) && ((z+k)>=0) && ((z+k)<sL[2]) ) { DNi = D[ni] + ND[n]*SEG[ind]; if ( DNi<D[ind] ) {D[ind]=DNi; L[ind]=L[ni]; } } } n++; } diff = abs2( Dio - D[ind] ); if ( maxdiffi<diff ) maxdiffi=diff; } } maxdiff = maxdiffi; } }
float abs (vector<N, T> const& v) { return std::sqrt(float(abs2(v))); }
double binarySearch(double* pdData, long N, double dItem, double dirIfFound, double dirIfNotFound) { long iPos = 0; double dPos = 0; bool foundItem = false; long lower = 1; long upper = N; long mid; while ((upper > lower+1) && (iPos == 0)) { mid = ((upper+lower)/2); if (pdData[mid] == dItem) { iPos = mid; foundItem = true; } else { if (pdData[mid] > dItem) upper = mid; else lower = mid; } } if (!foundItem) { /* didn't find during search: test upper & lower bounds */ if (pdData[upper] == dItem) { iPos = upper; foundItem = true; } else if (pdData[lower] == dItem) { iPos = lower; foundItem = true; } } if (foundItem) { if (dirIfFound == -1) while ((iPos > 1) && (pdData[iPos-1] == pdData[iPos])) iPos--; else if (dirIfFound == +1) while ((iPos < N) && (pdData[iPos+1] == pdData[iPos])) iPos++; } else if (!foundItem) { if (dirIfNotFound == -1) { if (dItem > pdData[upper]) // this could be true if upper is at the end of the array iPos = upper; else iPos = lower; } else if (dirIfNotFound == 0) { iPos = 0; } else if (dirIfNotFound == 0.5) { dPos = lower + (dItem-pdData[lower])/(pdData[upper]-pdData[lower]); } else if (dirIfNotFound == 1) { if (dItem < pdData[lower]) // this could be true if lower is at the start of the array iPos = lower; else iPos = upper; } else if (dirIfNotFound == 2) { if (abs2(pdData[upper]-dItem) < abs2(pdData[lower]-dItem)) iPos = upper; else iPos = lower; } } if (dPos == 0) { // most of the time, except when dirIfNotFound = 0.5 dPos = iPos; } return dPos; }
/* Computes nonlinear propagation according to the following equations: * * Elliptical Equivalent: * dua/dz = (-j*gamma/3)*[(2+cos(2X)^2*|ua|^2 + (2+2sin(2X)^2)*|ub|^2] * ua * dub/dz = (-j*gamma/3)*[(2+cos(2X)^2*|ub|^2 + (2+2sin(2X)^2)*|ua|^2] * ub * * Circular Equivalent: * dua/dz = (-j*2*gamma/3)*(|ua|^2 + 2*|ub|^2)*ua * dub/dz = (-j*2*gamma/3)*(|ub|^2 + 2*|ua|^2)*ub */ void nonlinear_propagate(COMPLEX* uva,COMPLEX* uvb,COMPLEX* uahalf, COMPLEX* ubhalf,COMPLEX* u0a,COMPLEX* u0b, COMPLEX* u1a,COMPLEX* u1b,REAL gamma,REAL dz, REAL chi, int nt) { int jj; REAL coef,twoPcos,twoPsin; coef = (REAL) ((1.0/3.0)*gamma*dz); twoPcos = (2 + cos(2*chi)*cos(2*chi)) / 2; twoPsin = (2 + 2*sin(2*chi)*sin(2*chi)) / 2; for(jj = 0; jj < nt; jj++) { uva[jj][0] = uahalf[jj][0]*cos(coef*( twoPcos*(abs2(&u0a[jj])+abs2(&u1a[jj])) + twoPsin*(abs2(&u0b[jj])+abs2(&u1b[jj])) )) + uahalf[jj][1]*sin(coef*( twoPcos*(abs2(&u0a[jj])+abs2(&u1a[jj])) + twoPsin*(abs2(&u0b[jj])+abs2(&u1b[jj])) )); uva[jj][1] = uahalf[jj][1]*cos(coef*( twoPcos*(abs2(&u0a[jj])+abs2(&u1a[jj])) + twoPsin*(abs2(&u0b[jj])+abs2(&u1b[jj])) )) - uahalf[jj][0]*sin(coef*( twoPcos*(abs2(&u0a[jj])+abs2(&u1a[jj])) + twoPsin*(abs2(&u0b[jj])+abs2(&u1b[jj])) )); uvb[jj][0] = ubhalf[jj][0]*cos(coef*( twoPcos*(abs2(&u0b[jj])+abs2(&u1b[jj])) + twoPsin*(abs2(&u0a[jj])+abs2(&u1a[jj])) )) + ubhalf[jj][1]*sin(coef*( twoPcos*(abs2(&u0b[jj])+abs2(&u1b[jj])) + twoPsin*(abs2(&u0a[jj])+abs2(&u1a[jj])) )); uvb[jj][1] = ubhalf[jj][1]*cos(coef*( twoPcos*(abs2(&u0b[jj])+abs2(&u1b[jj])) + twoPsin*(abs2(&u0a[jj])+abs2(&u1a[jj])) )) - ubhalf[jj][0]*sin(coef*( twoPcos*(abs2(&u0b[jj])+abs2(&u1b[jj])) + twoPsin*(abs2(&u0a[jj])+abs2(&u1a[jj])) )); } }
void neural_update(neural_state_t *n_s, float rin, int encoder_counter, int command_distance_count){ if (car_state != CAR_STATE_IDLE) { int i = 0, j = 0; // 1. RBFNN Output n_s->ynout = 0; for ( i = 0; i < neuralNumber; ++i) { n_s->norm_c_2[i] = pow2((n_s->x[0] - n_s->c[0][i]), 2) + pow2((n_s->x[1] - n_s->c[1][i]), 2) + pow2((n_s->x[2] - n_s->c[2][i]), 2); float tmp = (((-0.5) * n_s->norm_c_2[i])); tmp = tmp/ pow2(n_s->b[i], 2); n_s->h[i] = exponential(tmp); n_s->ynout = n_s->ynout + (n_s->h[i])*(n_s->w[i]); } n_s->ynout_sum += n_s->ynout; // 2 Get the motor speed of last clock cycle, and calculate the error of rbf n_s->erbf = encoder_counter - n_s->ynout; n_s->erbf_record[4] = n_s->erbf_record[3]; n_s->erbf_record[3] = n_s->erbf_record[2]; n_s->erbf_record[2] = n_s->erbf_record[1]; n_s->erbf_record[1] = n_s->erbf_record[0]; n_s->erbf_record[0] = abs2(n_s->erbf); n_s->erbf_avg = (n_s->erbf_record[0] + n_s->erbf_record[1] + n_s->erbf_record[2] + n_s->erbf_record[3] + n_s->erbf_record[4])/5.0; // 3. Update w of RBFNN for ( i = 0; i < neuralNumber; ++i) { float tmp = n_s->erbf * n_s->h[i]; n_s->dw[i] = n_s->eta * tmp; n_s->w_1[i] = n_s->w[i]; n_s->w[i] = n_s->w_1[i] + n_s->dw[i]; } // 4. Update bj for ( i = 0; i < neuralNumber; ++i) { float tmp = n_s->eta * n_s->erbf; tmp = tmp * n_s->w[i]; tmp = tmp * n_s->h[i]; tmp = tmp * n_s->norm_c_2[i]; tmp = tmp / pow2(n_s->b[i], 3); n_s->db[i] = tmp; n_s->b_1[i] = n_s->b[i]; n_s->b[i] = n_s->b_1[i] + n_s->db[i]; } // 5. Update Cj for ( i = 0; i < neuralNumber; ++i) { for ( j = 0; j < centerNumber; ++j) { float tmp = n_s->eta * n_s->erbf; tmp = tmp * n_s->w[i]; tmp = tmp * n_s->h[i]; tmp = tmp * (n_s->x[j] - n_s->c[j][i]); tmp = tmp / pow2(n_s->b[i], 2); n_s->dc[j][i] = tmp; n_s->c_1[j][i] = n_s->c[j][i]; n_s->c[j][i] = n_s->c_1[j][i] + n_s->dc[j][i]; } } // 6. Calculate Jacobian n_s->yu = 0; for ( i = 0; i < neuralNumber; ++i) { float tmp = n_s->w[i] * n_s->h[i]; float tmp2 = (-1) * n_s->x[0]; tmp = tmp * (tmp2 + n_s->c[0][i]); tmp = tmp / pow2(n_s->b[i], 2); n_s->yu = n_s->yu + tmp; } n_s->dyu = n_s->yu; // 6.2 Calculate the error with reference model n_s->rf_out = referenceModel(rin, n_s->rf_out_1); n_s->e = n_s->rf_out - (float)encoder_counter; n_s->total_err += abs2(n_s->e); n_s->rf_out_2 = n_s->rf_out_1; n_s->rf_out_1 = n_s->rf_out; n_s->desire = n_s->desire +(int)n_s->rf_out; // 8. Incremental PID n_s->xc[0] = n_s->e - n_s->e_1; n_s->xc[1] = n_s->e; n_s->xc[2] = n_s->e - (2 * n_s->e_1) + n_s->e_2; //int tmp_erbf = (int)(abs2(erbf)*100); int tmp_erbf = (command_distance_count - n_s -> ynout_sum)/command_distance_count; tmp_erbf = tmp_erbf * 100; if (tmp_erbf < 10) { n_s -> erbf_correct_times ++; } if ((tmp_erbf < 10) && (encoder_counter > 1) && (n_s->erbf_correct_times >30 && n_s->erbf_avg < 1)){ n_s->erbf_correct_times ++; float kp_add = n_s->eta * n_s->e; kp_add = kp_add * n_s->dyu; kp_add = kp_add * n_s->xc[0]; float ki_add = n_s->eta * n_s->e; ki_add = ki_add * n_s->dyu; ki_add = ki_add * n_s->xc[1]; float kd_add = n_s->eta * n_s->e; kd_add = kd_add * n_s->dyu; kd_add = kd_add * n_s->xc[2]; // 10. update kp(k-1) ki(k-1) kd(k-1) n_s->kp_1 = n_s->kp; n_s->ki_1 = n_s->ki; n_s->kd_1 = n_s->kd; // 9. Update the parameter of PID controller if (n_s->stop_tune == 0 && car_state == CAR_STATE_MOVE_FORWARD){ n_s->kp = n_s->kp_1 + kp_add; n_s->ki = n_s->ki_1 + ki_add; // n_s->kd = n_s->kd_1 + kd_add; if (n_s->kp <= 0) { n_s->kp = 0; } if (n_s->ki <= 0) { n_s->ki = 0; } if (n_s->kd <= 0) { //n_s->kd = 0; } } } // 11. Calculate the output of PID controller n_s->du = n_s->kp * n_s->xc[0] + n_s->ki * n_s->xc[1] + n_s->kd * n_s->xc[2]; n_s->u = n_s->u_1 + n_s->du; if (n_s->u < 0) { n_s->u = 0; }else if(n_s->u > 110) { n_s->u = 110; } // 12. update yout(k-1) u(k-1) n_s->yout_1 = n_s->x[1]; n_s->u_2 = n_s->u_1; n_s->u_1 = n_s->u; // 13. update e(k-1) e(k-2) n_s->e_2 = n_s->e_1; n_s->e_1 = n_s->e; // 14. update input of RBFNN n_s->x[0] = n_s->du; n_s->x[1] = (float)encoder_counter; n_s->x[2] = n_s->yout_1; } }
T abs(const quaternion<T> &c) { return sqrt(abs2(c)); }
LocalCS_Data::quaternion::quaternion(const float rot[9], const quaternion *pq) { if (rot[0] == 1.0 && rot[4] == 1.0 && rot[8] == 1.0) { _a = 1.0; _b = _c = _d = 0.0; } else { float cos_theta = (rot[0] + rot[4] + rot[8] - 1.0) * 0.5; float stuff = (cos_theta + 1.0) * 0.5; float cos_theta_sur_2 = sqrt(stuff); float sin_theta_sur_2 = sqrt(1 - stuff); float x; float y; float z; find_invariant_vector(rot, x, y, z); float u; float v; float w; find_orthogonal_vector(x, y, z, u, v, w); float r; float s; float t; find_vector_for_BOD(x, y, z, u, v, w, r, s, t); float ru = rot[0] * u + rot[1] * v + rot[2] * w; float rv = rot[3] * u + rot[4] * v + rot[5] * w; float rw = rot[6] * u + rot[7] * v + rot[8] * w; float angle_sign_determinator = r * ru + s * rv + t * rw; if (angle_sign_determinator > 0.0) { _a = cos_theta_sur_2; _b = x * sin_theta_sur_2; _c = y * sin_theta_sur_2; _d = z * sin_theta_sur_2; } else if (angle_sign_determinator < 0.0) { _a = cos_theta_sur_2; _b = -x * sin_theta_sur_2; _c = -y * sin_theta_sur_2; _d = -z * sin_theta_sur_2; } else if (u * ru + v * rv + w * rw < 0.0) { _a = 0.0; _b = x; _c = y; _d = z; } else { _a = 1.0; _b = _c = _d = 0.0; } } if (pq && abs2(*pq, *this) > abs2(*this, *pq)) { _a *= -1.0; _b *= -1.0; _c *= -1.0; _d *= -1.0; } }
inline float2 const operator / ( float2 const& lhs, float2 const& rhs ) { float const c2d2 = abs2( rhs ); return float2{ (lhs.x*rhs.x+lhs.y*rhs.y)/c2d2, (-lhs.x*rhs.y+lhs.y*rhs.x)/c2d2 }; }
bool operator > ( float2 const& lhs, float2 const& rhs ) { return abs2( lhs ) > abs2( rhs ); }
static const xreal xnorm(const xcomplex &newz) { return _double(abs2(newz)); }
void fe_zc(hls::stream< ap_uint<32> > sampleFifo, hls::stream< ap_uint<32> > featureFifo, ap_uint<8> windowSize, ap_uint<32> threshold) { #pragma HLS INTERFACE ap_ctrl_none port=return #pragma HLS INTERFACE ap_fifo port=featureFifo #pragma HLS INTERFACE ap_fifo port=sampleFifo ap_uint<32> data; ap_int<16> sampleChannel1 = 0; ap_int<16> sampleChannel2 = 0; ap_uint<32> zcChannel1 = 0; ap_uint<32> zcChannel2 = 0; ap_int<2> stateChannel1 = 0; ap_int<2> stateChannel2 = 0; ap_uint<8> cntSamples = 0; // Wait for Samples to arrive in FIFO while( windowSize == 0 ) { } while(1) { zcChannel1 = 0; zcChannel2 = 0; stateChannel1 = 0; stateChannel2 = 0; // Count zero-crossing for channel 1 & 2 for(cntSamples=0; cntSamples < windowSize; cntSamples++) { // Read data from Sample-FIFO // 2 16 bit Samples at one position in 32 bit FIFO => Process 2 channels in parallel data = sampleFifo.read(); sampleChannel1 = data(15, 0); if( abs2(sampleChannel1) < threshold ) { sampleChannel1 = 0; } sampleChannel2 = data(31, 16); if( abs2(sampleChannel2) < threshold ) { sampleChannel2 = 0; } // Check whether a zero-crossing occurred or not // Channel 1 if( stateChannel1 == 0 ) { if( sampleChannel1 < 0 ) { stateChannel1 = -1; } else if( sampleChannel1 == 0 ) { stateChannel1 = 0; } else { stateChannel1 = 1; } } else if( stateChannel1 < 0 ) { if( sampleChannel1 > 0 ) { zcChannel1++; if( sampleChannel1 < 0 ) { stateChannel1 = -1; } else if( sampleChannel1 == 0 ) { stateChannel1 = 0; } else { stateChannel1 = 1; } } } else if( stateChannel1 > 0 ) { if( sampleChannel1 < 0 ) { zcChannel1++; if( sampleChannel1 < 0 ) { stateChannel1 = -1; } else if( sampleChannel1 == 0 ) { stateChannel1 = 0; } else { stateChannel1 = 1; } } } // Channel 2 if( stateChannel2 == 0 ) { if( sampleChannel2 < 0 ) { stateChannel2 = -1; } else if( sampleChannel2 == 0 ) { stateChannel2 = 0; } else { stateChannel2 = 1; } } else if( stateChannel2 < 0 ) { if( sampleChannel2 > 0 ) { zcChannel2++; if( sampleChannel2 < 0 ) { stateChannel2 = -1; } else if( sampleChannel2 == 0 ) { stateChannel2 = 0; } else { stateChannel2 = 1; } } } else if( stateChannel2 > 0 ) { if( sampleChannel2 < 0 ) { zcChannel2++; if( sampleChannel2 < 0 ) { stateChannel2 = -1; } else if( sampleChannel2 == 0 ) { stateChannel2 = 0; } else { stateChannel2 = 1; } } } } // Write back features to Feature-FIFO featureFifo.write(zcChannel1); featureFifo.write(zcChannel2); } }
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { REAL scale; /* scale factor */ REAL dt; /* time step */ REAL dz; /* propagation stepsize */ int nz; /* number of z steps to take */ int nalpha; /* number of beta coefs */ double* alphap; /* alpha(w) array, if applicable */ int nbeta; /* number of beta coefs */ double* beta; /* dispersion polynomial coefs */ REAL gamma; /* nonlinearity coefficient */ REAL traman = 0; /* Raman response time */ REAL toptical = 0; /* Optical cycle time = lambda/c */ int maxiter = 4; /* max number of iterations */ REAL tol = 1e-5; /* convergence tolerance */ REAL* w; /* vector of angular frequencies */ int iz,ii,jj; /* loop counters */ REAL phase, alpha, wii, fii; /* temporary variables */ COMPLEX nlp, /* nonlinear phase */ *ua, *ub, *uc; /* samples of u at three adjacent times */ char argstr[100]; /* string argument */ if (nrhs == 1) { if (mxGetString(prhs[0],argstr,100)) mexErrMsgTxt("Unrecognized option."); if (!strcmp(argstr,"-savewisdom")) { sspropc_save_wisdom(); } else if (!strcmp(argstr,"-forgetwisdom")) { FORGET_WISDOM(); } else if (!strcmp(argstr,"-loadwisdom")) { sspropc_load_wisdom(); } else if (!strcmp(argstr,"-patient")) { method = FFTW_PATIENT; } else if (!strcmp(argstr,"-exhaustive")) { method = FFTW_EXHAUSTIVE; } else if (!strcmp(argstr,"-measure")) { method = FFTW_MEASURE; } else if (!strcmp(argstr,"-estimate")) { method = FFTW_ESTIMATE; } else mexErrMsgTxt("Unrecognized option."); return; } if (nrhs < 7) mexErrMsgTxt("Not enough input arguments provided."); if (nlhs > 1) mexErrMsgTxt("Too many output arguments."); sspropc_initialize_data(mxGetNumberOfElements(prhs[0])); /* parse input arguments */ dt = (REAL) mxGetScalar(prhs[1]); dz = (REAL) mxGetScalar(prhs[2]); nz = round(mxGetScalar(prhs[3])); nalpha = mxGetNumberOfElements(prhs[4]); alphap = mxGetPr(prhs[4]); beta = mxGetPr(prhs[5]); nbeta = mxGetNumberOfElements(prhs[5]); gamma = (REAL) mxGetScalar(prhs[6]); if (nrhs > 7) traman = (mxIsEmpty(prhs[7])) ? 0 : (REAL) mxGetScalar(prhs[7]); if (nrhs > 8) toptical = (mxIsEmpty(prhs[8])) ? 0 : (REAL) mxGetScalar(prhs[8]); if (nrhs > 9) maxiter = (mxIsEmpty(prhs[9])) ? 4 : round(mxGetScalar(prhs[9])); if (nrhs > 10) tol = (mxIsEmpty(prhs[10])) ? 1e-5 : (REAL) mxGetScalar(prhs[10]); if ((nalpha != 1) && (nalpha != nt)) mexErrMsgTxt("Invalid vector length (alpha)."); /* compute vector of angular frequency components */ /* MATLAB equivalent: w = wspace(tv); */ w = (REAL*)mxMalloc(sizeof(REAL)*nt); for (ii = 0; ii <= (nt-1)/2; ii++) { w[ii] = 2*pi*ii/(dt*nt); } for (; ii < nt; ii++) { w[ii] = 2*pi*ii/(dt*nt) - 2*pi/dt; } /* compute halfstep and initialize u0 and u1 */ for (jj = 0; jj < nt; jj++) { if (nbeta != nt) for (ii = 0, phase = 0, fii = 1, wii = 1; ii < nbeta; ii++, fii*=ii, wii*=w[jj]) phase += wii*((REAL)beta[ii])/fii; else phase = (REAL)beta[jj]; alpha = (nalpha == nt) ? (REAL)alphap[jj] : (REAL)alphap[0]; halfstep[jj][0] = +exp(-alpha*dz/4)*cos(phase*dz/2); halfstep[jj][1] = -exp(-alpha*dz/4)*sin(phase*dz/2); u0[jj][0] = (REAL) mxGetPr(prhs[0])[jj]; u0[jj][1] = mxIsComplex(prhs[0]) ? (REAL)(mxGetPi(prhs[0])[jj]) : 0.0; u1[jj][0] = u0[jj][0]; u1[jj][1] = u0[jj][1]; } mxFree(w); /* free w vector */ mexPrintf("Performing split-step iterations ... "); EXECUTE(p1); /* ufft = fft(u0) */ for (iz = 0; iz < nz; iz++) { cmult(uhalf,halfstep,ufft); /* uhalf = halfstep.*ufft */ EXECUTE(ip1); /* uhalf = nt*ifft(uhalf) */ for (ii = 0; ii < maxiter; ii++) { if ((traman == 0.0) && (toptical == 0)) { for (jj = 0; jj < nt; jj++) { phase = gamma*(u0[jj][0]*u0[jj][0] + u0[jj][1]*u0[jj][1] + u1[jj][0]*u1[jj][0] + u1[jj][1]*u1[jj][1])*dz/2; uv[jj][0] = (uhalf[jj][0]*cos(phase) + uhalf[jj][1]*sin(phase))/nt; uv[jj][1] = (-uhalf[jj][0]*sin(phase) + uhalf[jj][1]*cos(phase))/nt; } } else { jj = 0; ua = &u0[nt-1]; ub = &u0[jj]; uc = &u0[jj+1]; nlp[1] = -toptical*(abs2(uc) - abs2(ua) + prodr(ub,uc) - prodr(ub,ua))/(4*pi*dt); nlp[0] = abs2(ub) - traman*(abs2(uc) - abs2(ua))/(2*dt) + toptical*(prodi(ub,uc) - prodi(ub,ua))/(4*pi*dt); ua = &u1[nt-1]; ub = &u1[jj]; uc = &u1[jj+1]; nlp[1] += -toptical*(abs2(uc) - abs2(ua) + prodr(ub,uc) - prodr(ub,ua))/(4*pi*dt); nlp[0] += abs2(ub) - traman*(abs2(uc) - abs2(ua))/(2*dt) + toptical*(prodi(ub,uc) - prodi(ub,ua))/(4*pi*dt); nlp[0] *= gamma*dz/2; nlp[1] *= gamma*dz/2; uv[jj][0] = (uhalf[jj][0]*cos(nlp[0])*exp(+nlp[1]) + uhalf[jj][1]*sin(nlp[0])*exp(+nlp[1]))/nt; uv[jj][1] = (-uhalf[jj][0]*sin(nlp[0])*exp(+nlp[1]) + uhalf[jj][1]*cos(nlp[0])*exp(+nlp[1]))/nt; for (jj = 1; jj < nt-1; jj++) { ua = &u0[jj-1]; ub = &u0[jj]; uc = &u0[jj+1]; nlp[1] = -toptical*(abs2(uc) - abs2(ua) + prodr(ub,uc) - prodr(ub,ua))/(4*pi*dt); nlp[0] = abs2(ub) - traman*(abs2(uc) - abs2(ua))/(2*dt) + toptical*(prodi(ub,uc) - prodi(ub,ua))/(4*pi*dt); ua = &u1[jj-1]; ub = &u1[jj]; uc = &u1[jj+1]; nlp[1] += -toptical*(abs2(uc) - abs2(ua) + prodr(ub,uc) - prodr(ub,ua))/(4*pi*dt); nlp[0] += abs2(ub) - traman*(abs2(uc) - abs2(ua))/(2*dt) + toptical*(prodi(ub,uc) - prodi(ub,ua))/(4*pi*dt); nlp[0] *= gamma*dz/2; nlp[1] *= gamma*dz/2; uv[jj][0] = (uhalf[jj][0]*cos(nlp[0])*exp(+nlp[1]) + uhalf[jj][1]*sin(nlp[0])*exp(+nlp[1]))/nt; uv[jj][1] = (-uhalf[jj][0]*sin(nlp[0])*exp(+nlp[1]) + uhalf[jj][1]*cos(nlp[0])*exp(+nlp[1]))/nt; } /* we now handle the endpoint where jj = nt-1 */ ua = &u0[jj-1]; ub = &u0[jj]; uc = &u0[0]; nlp[1] = -toptical*(abs2(uc) - abs2(ua) + prodr(ub,uc) - prodr(ub,ua))/(4*pi*dt); nlp[0] = abs2(ub) - traman*(abs2(uc) - abs2(ua))/(2*dt) + toptical*(prodi(ub,uc) - prodi(ub,ua))/(4*pi*dt); ua = &u1[jj-1]; ub = &u1[jj]; uc = &u1[0]; nlp[1] += -toptical*(abs2(uc) - abs2(ua) + prodr(ub,uc) - prodr(ub,ua))/(4*pi*dt); nlp[0] += abs2(ub) - traman*(abs2(uc) - abs2(ua))/(2*dt) + toptical*(prodi(ub,uc) - prodi(ub,ua))/(4*pi*dt); nlp[0] *= gamma*dz/2; nlp[1] *= gamma*dz/2; uv[jj][0] = (uhalf[jj][0]*cos(nlp[0])*exp(+nlp[1]) + uhalf[jj][1]*sin(nlp[0])*exp(+nlp[1]))/nt; uv[jj][1] = (-uhalf[jj][0]*sin(nlp[0])*exp(+nlp[1]) + uhalf[jj][1]*cos(nlp[0])*exp(+nlp[1]))/nt; } EXECUTE(p2); /* uv = fft(uv) */ cmult(ufft,uv,halfstep); /* ufft = uv.*halfstep */ EXECUTE(ip2); /* uv = nt*ifft(ufft) */ if (ssconverged(uv,u1,tol)) { /* test for convergence */ cscale(u1,uv,1.0/nt); /* u1 = uv/nt; */ break; /* exit from ii loop */ } else { cscale(u1,uv,1.0/nt); /* u1 = uv/nt; */ } } if (ii == maxiter) mexWarnMsgTxt("Failed to converge."); cscale(u0,u1,1); /* u0 = u1 */ } mexPrintf("done.\n"); /* allocate space for returned vector */ plhs[0] = mxCreateDoubleMatrix(nt,1,mxCOMPLEX); for (jj = 0; jj < nt; jj++) { mxGetPr(plhs[0])[jj] = (double) u1[jj][0]; /* fill return vector */ mxGetPi(plhs[0])[jj] = (double) u1[jj][1]; /* with u1 */ } sspropc_destroy_data(); }
int apollonius_in(circ aa[], int ss[], int flip, int divert) { vec n[3], x[3], t[3], a, b, center; int s[3], iter = 0, res = 0; double diff = 1, diff_old = -1, axb, d, r; for3 { s[i] = ss[i] ? 1 : -1; x[i] = aa[i].c; } while (diff > 1e-20) { a = x[0] - x[2], b = x[1] - x[2]; diff = 0; axb = -cross(a, b); d = sqrt(abs2(a) * abs2(b) * abs2(a - b)); if (VERBOSE) { const char *z = 1 + "-0+"; printf("%c%c%c|%c%c|", z[s[0]], z[s[1]], z[s[2]], z[flip], z[divert]); printf(CPLX3, cp(x[0]), cp(x[1]), cp(x[2])); } /* r and center represent an arc through points x[i]. Each step, we'll deform this arc by pushing or pulling some point on it towards the edge of each given circle. */ r = fabs(d / (2 * axb)); center = (abs2(a)*b - abs2(b)*a) / (2 * axb) * I + x[2]; /* maybe the "arc" is actually straight line; then we have two choices in defining "push" and "pull", so try both */ if (!axb && flip != -1 && !divert) { if (!d) { /* generally means circle centers overlap */ printf("Given conditions confused me.\n"); return 0; } if (VERBOSE) puts("\n[divert]"); divert = 1; res = apollonius_in(aa, ss, -1, 1); } /* if straight line, push dir is its norm; else it's away from center */ for3 n[i] = axb ? aa[i].c - center : a * I * flip; for3 t[i] = aa[i].c + n[i] / cabs(n[i]) * aa[i].r * s[i]; /* diff: how much tangent points have moved since last iteration */ for3 diff += abs2(t[i] - x[i]), x[i] = t[i]; if (VERBOSE) printf(" %g\n", diff); /* keep an eye on the total diff: failing to converge means no solution */ if (diff >= diff_old && diff_old >= 0) if (iter++ > 20) return res; diff_old = diff; } printf("found: "); if (axb) printf("circle "CPLX", r = %f\n", cp(center), r); else printf("line "CPLX3"\n", cp(x[0]), cp(x[1]), cp(x[2])); return res + 1; }