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