//最大熵和法 int EdgeDetection::MESM(float p[256]) { int i,t,T; float p1,p2,h1,h,fei,tempfei; fei=-65535.0; for(t=0;t<255;t++){ //算熵和，并求出最大的熵和 p1=p2=h1=h=0.0; for(i=0;i<=t;i++) p1+=p[i]; p2=1-p1; for(i=0;i<=t;i++) if(p1>MINNUM) if(fabs(p[i]/p1)>MINNUM) h1+=-1*(p[i]/p1)*(float)(log(p[i]/p1)); for(i=0;i<=t;i++) if(fabs(p[i])>MINNUM) h+=-1*p[i]*(float)(log(p[i])); if(p1>MINNUM&&p2>MINNUM) tempfei=(float)(log(p1*p2))+h/p1+(h1-h)/p2; else tempfei=-65535.0; if(tempfei>fei) { fei=tempfei; T=t; } } return T; }

void turnToDirection(int turnDirection) { adjustAngle(); int currentDir; currentDir = currentDirection(); double target = (currentDir + turnDirection) * M_PI / 2; while(fabs(currentHeading() - target) > turnThreshold) { if(fabs(currentHeading() - target) < slowThreshold) { if(currentHeading() > target) { set_motors(-5,5); } if(currentHeading() < target) { set_motors(5,-5); } } else { if(currentHeading() > target) { set_motors(-15,15); } if(currentHeading() < target) { set_motors(15,-15); } } calcPos(); } set_motors(0,0); adjustAngle(); usleep(10000); }

//! Numerical recipe Chapiter 3 //! Polynomial interpolation void polint(std::vector<double> &xa, std::vector<double> &ya, const double x, double &y, double &dy) { int i,m,ns=0; double den,dif,dift,ho,hp,w; int n= (int)xa.size(); std::vector<double> c(n),d(n); dif=fabs(x-xa[0]); for (i=0;i<n;i++) { if ((dift=fabs(x-xa[i])) < dif) { ns=i; dif=dift; } c[i]=ya[i]; d[i]=ya[i]; } y=ya[ns--]; for (m=1;m<n;m++) { for (i=0;i<n-m;i++) { ho=xa[i]-x; hp=xa[i+m]-x; w=c[i+1]-d[i]; if ((den=ho-hp) == 0.0) std::cout << "Error in routine polint" << std::endl; den=w/den; d[i]=hp*den; c[i]=ho*den; } y += (dy=(2*(ns+1) < (n-m) ? c[ns+1] : d[ns--])); } }

void Pulsar::RobustMower::compute (PhaseWeight* mask, const Profile* difference) { const unsigned nbin = difference->get_nbin(); const float* amps = difference->get_amps(); std::vector<float> diff (nbin, 0.0); unsigned valid = 0; for (unsigned ibin=0; ibin<nbin; ibin++) if (difference->get_amps()[ibin] != 0.0) { diff[valid] = fabs( difference->get_amps()[ibin] ); valid ++; } #ifdef _DEBUG cerr << "nbin=" << nbin << " valid=" << valid << endl; #endif std::nth_element (diff.begin(), diff.begin()+valid/2, diff.begin()+valid); float madm = diff[ valid/2 ]; for (unsigned ibin=0; ibin<nbin; ibin++) (*mowed)[ibin] = ( fabs(amps[ibin]) > madm * cutoff_threshold ); }

float dfridr(float (*func)(float), float x, float h, float *err) { int i,j; float errt,fac,hh,**a,ans; if (h == 0.0) nrerror("h must be nonzero in dfridr."); a=matrix(1,NTAB,1,NTAB); hh=h; a[1][1]=((*func)(x+hh)-(*func)(x-hh))/(2.0*hh); *err=BIG; for (i=2; i<=NTAB; i++) { hh /= CON; a[1][i]=((*func)(x+hh)-(*func)(x-hh))/(2.0*hh); fac=CON2; for (j=2; j<=i; j++) { a[j][i]=(a[j-1][i]*fac-a[j-1][i-1])/(fac-1.0); fac=CON2*fac; errt=FMAX(fabs(a[j][i]-a[j-1][i]),fabs(a[j][i]-a[j-1][i-1])); if (errt <= *err) { *err=errt; ans=a[j][i]; } } if (fabs(a[i][i]-a[i-1][i-1]) >= SAFE*(*err)) break; } free_matrix(a,1,NTAB,1,NTAB); return ans; }

/* * Get shortest arc length and direction between two angles */ sREAL sGetCircularDistance(sREAL from, sREAL to) { sREAL result; /* clockwise distance */ sREAL cw_d = 0.0; /* counter-clockwise distance */ sREAL ccw_d= 0.0; if ((to >= 0.0 && from >= 0.0) || (to <= 0.0 && from <= 0.0)) result = to - from; else { if (from >= 0.0) { /* 0.0 > to > -Pi */ cw_d = Pi - from + Pi + to; ccw_d = to - from; } else { /* 0.0 > from > -Pi */ cw_d = to - from; ccw_d = -Pi - from + to - Pi; } if (fabs(cw_d) < fabs(ccw_d)) result = cw_d; else result = ccw_d; } return (result); }

void GetGradients(const double* score, score_t* gradients, score_t* hessians) const override { if (weights_ == nullptr) { #pragma omp parallel for schedule(static) for (data_size_t i = 0; i < num_data_; ++i) { // get label and label weights const int is_pos = is_pos_(label_[i]); const int label = label_val_[is_pos]; const double label_weight = label_weights_[is_pos]; // calculate gradients and hessians const double response = -label * sigmoid_ / (1.0f + std::exp(label * sigmoid_ * score[i])); const double abs_response = fabs(response); gradients[i] = static_cast<score_t>(response * label_weight); hessians[i] = static_cast<score_t>(abs_response * (sigmoid_ - abs_response) * label_weight); } } else { #pragma omp parallel for schedule(static) for (data_size_t i = 0; i < num_data_; ++i) { // get label and label weights const int is_pos = is_pos_(label_[i]); const int label = label_val_[is_pos]; const double label_weight = label_weights_[is_pos]; // calculate gradients and hessians const double response = -label * sigmoid_ / (1.0f + std::exp(label * sigmoid_ * score[i])); const double abs_response = fabs(response); gradients[i] = static_cast<score_t>(response * label_weight * weights_[i]); hessians[i] = static_cast<score_t>(abs_response * (sigmoid_ - abs_response) * label_weight * weights_[i]); } } }

static void chk_coords(int frame, int natoms, rvec *x, matrix box, real fac, real tol) { int i, j; int nNul = 0; real vol = det(box); for (i = 0; (i < natoms); i++) { for (j = 0; (j < DIM); j++) { if ((vol > 0) && (fabs(x[i][j]) > fac*box[j][j])) { printf("Warning at frame %d: coordinates for atom %d are large (%g)\n", frame, i, x[i][j]); } } if ((fabs(x[i][XX]) < tol) && (fabs(x[i][YY]) < tol) && (fabs(x[i][ZZ]) < tol)) { nNul++; } } if (nNul > 0) { printf("Warning at frame %d: there are %d particles with all coordinates zero\n", frame, nNul); } }

static XY s_forward (LP lp, PJ *P) { /* Spheroidal, forward */ XY xy = {0.0,0.0}; if (fabs (fabs(lp.phi) - HALFPI) <= EPS10) F_ERROR; xy.x = lp.lam; xy.y = tan(lp.phi); return xy; }

void FourWheelOmniPilot::updateErrors(float x_goal, float y_goal) { // Reset queues if the target has moved if(Measurements::distance(Point(x_goal,y_goal), prev_goal_target) > 50) clearErrors(); //Integral Error for distance if (dist_error_deque.size() == DIST_ERROR_MAXSIZE) { double popped_integral = dist_error_integral - dist_error_deque.front(); double sign_popped_integral = (popped_integral > 0) - (popped_integral < 0); dist_error_integral = std::min(1000000.0, fabs(popped_integral)) * sign_popped_integral; dist_error_deque.pop_front(); } double pushed_integral = dist_error_integral + distance_error; double sign_pushed_integral = (pushed_integral > 0) - (pushed_integral < 0); dist_error_integral = std::min(1000000.0, fabs(pushed_integral)) * sign_pushed_integral; dist_error_deque.push_back(distance_error); //Integral Error for orientation if (angle_error_deque.size() == ANGLE_ERROR_MAXSIZE) { angle_error_integral -= angle_error_deque.front(); angle_error_deque.pop_front(); } angle_error_integral += angle_error; angle_error_deque.push_back(angle_error); //std::cout << "DIST ERR INT: " << dist_error_integral << std::endl; }

/*! Attempts to maintain a list of unique solutions. Therefore, whenever a new state is added to the list, we check if any of the states that are already in the list are within a given distance of the new state. If so, the best one is kept and the other one is thrown away. This method does not gurantee unique states, but it comes close and runs in linear time for each addition, rather than square time for maintenance. */ bool EGPlanner::addToListOfUniqueSolutions(GraspPlanningState *s, std::list<GraspPlanningState *> *list, double distance) { std::list<GraspPlanningState *>::iterator it; it = list->begin(); bool add = true; while (it != list->end()) { double d = stateDistance(s, *it); if (fabs(d) < distance) { DBGP("Distance: " << fabs(d)); //states are close to each other if (s->getEnergy() < (*it)->getEnergy()) { //new state is better; remove old one from list delete(*it); it = list->erase(it); DBGP("Old state removed"); } else { //old state is better; we don't want to add the new one add = false; break; } } else { //states are not close, proceed through the list it++; } } if (add) { list->push_back(s); } return add; }

bool iupPlot::FindDataSetSample(double inX, double inY, int &outIndex, const char* &outName, int &outSampleIndex, double &outX, double &outY, const char* &outStrX) const { if (!mAxisX.mTrafo || !mAxisY.mTrafo) return false; double theX = mAxisX.mTrafo->TransformBack(inX); double theY = mAxisY.mTrafo->TransformBack(inY); double tolX = (fabs(mAxisX.mMax - mAxisX.mMin) / mViewport.mWidth) * 5.0; // 5 pixels tolerance double tolY = (fabs(mAxisY.mMax - mAxisY.mMin) / mViewport.mHeight) * 5.0; for (int ds = 0; ds < mDataSetListCount; ds++) { iupPlotDataSet* dataset = mDataSetList[ds]; if (dataset->FindSample(theX, theY, tolX, tolY, outSampleIndex, outX, outY)) { const iupPlotDataBase *theXData = dataset->GetDataX(); if (theXData->IsString()) { const iupPlotDataString *theStringXData = (const iupPlotDataString *)(theXData); outStrX = theStringXData->GetSampleString(outSampleIndex); } else outStrX = NULL; outIndex = ds; outName = dataset->GetName(); return true; } } return false; }

size_t PointVec::uniqueInsert (Point* pnt) { size_t n (_data_vec->size()), k; const double eps (std::numeric_limits<double>::epsilon()); for (k = 0; k < n; k++) if (fabs((*((*_data_vec)[k]))[0] - (*pnt)[0]) < eps && fabs( (*((*_data_vec)[k]))[1] - (*pnt)[1]) < eps && fabs( (*((*_data_vec)[k]))[2] - (*pnt)[2]) < eps) break; if(k == n) { _data_vec->push_back (pnt); // update bounding box _aabb.update (*((*_data_vec)[n])); // update shortest distance for (size_t i(0); i < n; i++) { double sqr_dist (MathLib::sqrDist((*_data_vec)[i], (*_data_vec)[n])); if (sqr_dist < _sqr_shortest_dist) _sqr_shortest_dist = sqr_dist; } return n; } delete pnt; pnt = NULL; return k; }

double Mod2(complex c) { //This ckeck is required to avoid floating point underflow!! if(fabs(c.re) < 1E-30) c.re = 0; if(fabs(c.im) < 1E-30) c.im = 0; return c.re * c.re + c.im * c.im; }

/** * Deconvolves a Gaussian "beam" from a Gaussian component. * Routine translated from the AIPSish APL/SUB/DECONV.FOR/DECONV * \param fmaj Convolved major axis * \param fmin Convolved minor axis * \param fpa Convolved position angle of major axis * \param cmaj Beam major axis * \param cmin Beam minor axis * \param cpa Beam position angle of major axis * \param rmaj [out] Actual major axis; = 0 => unable to fit * \param rmin [out] Actual minor axis; = 0 => unable to fit * \param rpa [out] Actual position angle of major axis */ void ObitConvUtilDeconv (ofloat fmaj, ofloat fmin, ofloat fpa, ofloat cmaj, ofloat cmin, ofloat cpa, ofloat *rmaj, ofloat *rmin, ofloat *rpa) { ofloat cmj2, cmn2, fmj2, fmn2, sinc, cosc, rhoc, sigic2, det, rhoa, lfpa, lcpa, konst = 28.647888; olong csux; /* Get useful constants */ csux = (olong) ((fpa+900.0)/180.0); lfpa = (fpa+900.0) - csux*180.0; csux = (olong) ((cpa+900.0)/180.0); lcpa = (cpa+900.0) - csux*180.0; cmj2 = cmaj * cmaj; cmn2 = cmin * cmin; fmj2 = fmaj * fmaj; fmn2 = fmin * fmin; sinc = (lfpa - lcpa) / konst; cosc = cos (sinc); sinc = sin (sinc); /* Trigonometry now */ rhoc = (fmj2 - fmn2) * cosc - (cmj2 - cmn2); if (rhoc == 0.0) { sigic2 = 0.0; rhoa = 0.0; } else { sigic2 = atan((fmj2 - fmn2) * sinc / rhoc); rhoa = ((cmj2 - cmn2) - (fmj2 - fmn2) * cosc) / (2.0 * cos (sigic2)); } (*rpa) = sigic2 * konst + lcpa; det = ((fmj2 + fmn2) -(cmj2 + cmn2)) / 2.0; (*rmaj) = det - rhoa; (*rmin) = det + rhoa; /* Swap to get major > minor */ (*rmaj) = MAX (0.0, *rmaj); (*rmin) = MAX (0.0, *rmin); (*rmaj) = sqrt (fabs (*rmaj)); (*rmin) = sqrt (fabs (*rmin)); if (*rmaj < *rmin) { sinc = (*rmaj); (*rmaj) = (*rmin); (*rmin) = sinc; (*rpa) = (*rpa)+90.0; } /* Fix up PA */ csux = (olong) ((*rpa+900.0)/180.0); *rpa = (*rpa+900.0) - csux*180.0; if (*rmaj == 0.0) { (*rpa) = 0.0; } else if (*rmin == 0.0) { if ((fabs(*rpa-lfpa) > 45.0) && (fabs(*rpa-lfpa) < 135.0)) { csux = (olong) ((*rpa+450.0)/180.0); *rpa = (*rpa+450.0) - csux*180.0; } } } /* end of routine ObitConvUtilDeconv */

void BoxCover::Update(GuiTrigger * t) { s8 movY = t->WPAD_Stick((t->wpad.exp.type == WPAD_EXP_CLASSIC), 0); s8 movX = t->WPAD_Stick((t->wpad.exp.type == WPAD_EXP_CLASSIC), 1); //! Drop stick moves of less than 10 because of sensitivity if(fabs(movY) < 10.0f) movY = 0; if(fabs(movX) < 10.0f) movX = 0; if(movY != 0 || movX != 0) last_manual_move_frame = frameCount; RotY += (f32) movY / 50.0f; RotX -= (f32) movX / 50.0f; if(Zoomable) WiiPADControl(t); //! Stop movement for about 5 sec after manual move if(frameCount-last_manual_move_frame < 250) return; Animation = sinf(DegToRad(AnimRotate))*2.0f; Animation2 = cosf(DegToRad(AnimRotate))*5.0f; AnimRotate += 0.1f; if(AnimRotate > 360.0f) AnimRotate = 0.0f; }

// 0:in 1:out 2:not sure unsigned int CContactTarget3D_Mesh::FindInOut_IntersectionRay (double px, double py, double pz, const double dir[3]) const { double p0[3] = {px,py,pz}; double p1[3] = {px+dir[0],py+dir[1],pz+dir[2]}; unsigned int icnt = 0; for(unsigned int itri=0;itri<ntri_;itri++){ unsigned int i1 = aTri_[itri*3+0]; unsigned int i2 = aTri_[itri*3+1]; unsigned int i3 = aTri_[itri*3+2]; const double v0 = Com::TetVolume3D(p0, pXYZs_+i1*3, pXYZs_+i2*3, pXYZs_+i3*3); const double sign = ( v0 > 0 ) ? 1 : -1; const double v1 = Com::TetVolume3D(p0, pXYZs_+i2*3, pXYZs_+i3*3, p1)*sign; const double v2 = Com::TetVolume3D(p0, pXYZs_+i3*3, pXYZs_+i1*3, p1)*sign; const double v3 = Com::TetVolume3D(p0, pXYZs_+i1*3, pXYZs_+i2*3, p1)*sign; if( fabs(v1+v2+v3) < 1.0e-10 ) return 2; // p0 and p1 is on the triangle double inv_v4 = 1.0/fabs(v1+v2+v3); const double r1 = v1*inv_v4; const double r2 = v2*inv_v4; const double r3 = v3*inv_v4; const double tol = 1.0e-2; if( r1 < -tol || r2 < -tol || r3 < -tol ) continue; // need tol ( compare with fabs(v1+v2+v3)? ) if( r1 < tol || r2 < tol || r3 < tol ) return 2; // on the edge double dir2[3] = { pXYZs_[i1*3+0]*r1 + pXYZs_[i2*3+0]*r2 + pXYZs_[i3*3+0]*r3 - px, pXYZs_[i1*3+1]*r1 + pXYZs_[i2*3+1]*r2 + pXYZs_[i3*3+1]*r3 - py, pXYZs_[i1*3+2]*r1 + pXYZs_[i2*3+2]*r2 + pXYZs_[i3*3+2]*r3 - pz}; double dotdir = Com::Dot3D(dir,dir2); if( dotdir > 0 ) icnt++; } if( icnt % 2 == 0 ) return 1; return 0; }

void MyInbetweener::stabilizeSegments(TStroke *stroke) { for (int j = 0; j + 4 < stroke->getControlPointCount(); j += 4) { TThickPoint q0 = stroke->getControlPoint(j); TThickPoint q4 = stroke->getControlPoint(j + 4); TPointD p0 = convert(q0); TPointD p1 = convert(stroke->getControlPoint(j + 1)); TPointD p2 = convert(stroke->getControlPoint(j + 2)); TPointD p3 = convert(stroke->getControlPoint(j + 3)); TPointD p4 = convert(q4); TPointD v = normalize(p4 - p0); TPointD u = rotate90(v); double eps = tdistance(p0, p4) * 0.1; if (fabs(u * (p2 - p0)) < eps && fabs(u * (p1 - p0)) < eps && fabs(u * (p3 - p0)) < eps) { double e = 0.001; double d2 = norm2(p4 - p0); if (e * e * 6 * 6 > d2) e = sqrt(d2) / 6; TThickPoint q1(p0 + v * e, q0.thick); TThickPoint q3(p4 - v * e, q4.thick); stroke->setControlPoint(j + 1, q1); stroke->setControlPoint(j + 3, q3); } } }

double Vector_angle(const Vector vec0, const Vector vec1) { double scalp; scalp = Vector_dot(vec0, vec1); double lenp; lenp = (vec0.len * vec1.len); assert(lenp >= 0); if (Util_is_zero(lenp)) { return 0.0; } if (scalp != scalp) { Util_runtime_error("Vector_angle: Nan scalp"); } if ( Util_equal(scalp, lenp) || Util_equal(scalp, -lenp) || Util_equal(-scalp, lenp) ) { return 0.0; } double div; div = scalp / lenp; if (fabs(div - 1.0) < EPS || fabs(div + 1.0) < EPS) return 0.0; if (!Util_in_range_strict(-1.0, 1.0, div)) { Util_runtime_error("Vector_angle: Outside acos range"); } return acos(div); }

int gsl_sf_bessel_J0_e(const double x, gsl_sf_result * result) { double y = fabs(x); /* CHECK_POINTER(result) */ if(y < 2.0*GSL_SQRT_DBL_EPSILON) { result->val = 1.0; result->err = y*y; return GSL_SUCCESS; } else if(y <= 4.0) { return cheb_eval_e(&bj0_cs, 0.125*y*y - 1.0, result); } else { const double z = 32.0/(y*y) - 1.0; gsl_sf_result ca; gsl_sf_result ct; gsl_sf_result cp; const int stat_ca = cheb_eval_e(&_gsl_sf_bessel_amp_phase_bm0_cs, z, &ca); const int stat_ct = cheb_eval_e(&_gsl_sf_bessel_amp_phase_bth0_cs, z, &ct); const int stat_cp = gsl_sf_bessel_cos_pi4_e(y, ct.val/y, &cp); const double sqrty = sqrt(y); const double ampl = (0.75 + ca.val) / sqrty; result->val = ampl * cp.val; result->err = fabs(cp.val) * ca.err/sqrty + fabs(ampl) * cp.err; result->err += GSL_DBL_EPSILON * fabs(result->val); return GSL_ERROR_SELECT_3(stat_ca, stat_ct, stat_cp); } }

/* * Waveshape */ void OscilGen::waveshape() { int i; oldwaveshapingfunction = Pwaveshapingfunction; oldwaveshaping = Pwaveshaping; if(Pwaveshapingfunction == 0) return; oscilFFTfreqs.c[0] = 0.0; //remove the DC //reduce the amplitude of the freqs near the nyquist for(i = 1; i < OSCIL_SIZE / 8; i++) { REALTYPE tmp = i / (OSCIL_SIZE / 8.0); oscilFFTfreqs.s[OSCIL_SIZE / 2 - i] *= tmp; oscilFFTfreqs.c[OSCIL_SIZE / 2 - i] *= tmp; } fft->freqs2smps(oscilFFTfreqs, tmpsmps); //Normalize REALTYPE max = 0.0; for(i = 0; i < OSCIL_SIZE; i++) if(max < fabs(tmpsmps[i])) max = fabs(tmpsmps[i]); if(max < 0.00001) max = 1.0; max = 1.0 / max; for(i = 0; i < OSCIL_SIZE; i++) tmpsmps[i] *= max; //Do the waveshaping waveshapesmps(OSCIL_SIZE, tmpsmps, Pwaveshapingfunction, Pwaveshaping); fft->smps2freqs(tmpsmps, oscilFFTfreqs); //perform FFT }

void Particle::update(float ticks) { totalTicks += ticks; ticks *= 20; position.x += movementVector.x * ticks; position.y += movementVector.y * ticks; position.z += movementVector.z * ticks; movementVector.y = movementVector.y - ticks/10; movementVector.x = movementVector.x * (1 - ticks/50); movementVector.y = movementVector.y * (1 - ticks/50); movementVector.z = movementVector.z * (1 - ticks/50); if (position.y < -1) { position.y = -1 - (position.y+1); movementVector.y = 0 - movementVector.y; movementVector.x *= 0.55; movementVector.y *= 0.55; movementVector.z *= 0.55; } // Stop vibrating particles on the ground if (fabs(movementVector.x) < 0.01 && fabs(movementVector.y) < 0.01 && fabs(movementVector.z) < 0.01 && fabs(position.y + 1) < 0.01) { movementVector.x = 0; movementVector.y = 0; movementVector.z = 0; } }

Steer SteerAI::Seek(const Point& point) { Steer s; // advance memory pipeline: az[2] = az[1]; az[1] = az[0]; el[2] = el[1]; el[1] = el[0]; // approach if (point.z > 0.0f) { az[0] = atan2(fabs(point.x), point.z) * seek_gain; el[0] = atan2(fabs(point.y), point.z) * seek_gain; if (point.x < 0) az[0] = -az[0]; if (point.y > 0) el[0] = -el[0]; s.yaw = az[0] - seek_damp * (az[1] + az[2] * 0.5); s.pitch = el[0] - seek_damp * (el[1] + el[2] * 0.5); } // reverse else { if (point.x > 0) s.yaw = 1.0f; else s.yaw = -1.0f; s.pitch = -point.y * 0.5f; } seeking = 1; return s; }

double CLaplace::BagImprovement ( double *adY, double *adMisc, double *adOffset, double *adWeight, double *adF, double *adFadj, bool *afInBag, double dStepSize, unsigned long nTrain ) { double dReturnValue = 0.0; double dF = 0.0; double dW = 0.0; unsigned long i = 0; for(i=0; i<nTrain; i++) { if(!afInBag[i]) { dF = adF[i] + ((adOffset==NULL) ? 0.0 : adOffset[i]); dReturnValue += adWeight[i]*(fabs(adY[i]-dF) - fabs(adY[i]-dF-dStepSize*adFadj[i])); dW += adWeight[i]; } } return dReturnValue/dW; }

void turn90() { while( fabs(currentHeading() - (M_PI/2.0)) > turnThreshold) { if(fabs(currentHeading() - (M_PI/2.0)) < slowThreshold) { if(currentHeading() > (M_PI/2.0)) { set_motors(-1,1); } if(currentHeading() < (M_PI/2.0)) { set_motors(1,-1); } } else { if(currentHeading() > (M_PI/2.0)) { set_motors(-15,15); } if(currentHeading() < (M_PI/2.0)) { set_motors(15,-15); } } calcPos(); } set_motors(0,0); printf("Target : %f\n", M_PI/2.0); }

double CLaplace::Deviance ( double *adY, double *adMisc, double *adOffset, double *adWeight, double *adF, unsigned long cLength, int cIdxOff ) { unsigned long i=0; double dL = 0.0; double dW = 0.0; if(adOffset == NULL) { for(i=cIdxOff; i<cLength+cIdxOff; i++) { dL += adWeight[i]*fabs(adY[i]-adF[i]); dW += adWeight[i]; } } else { for(i=cIdxOff; i<cLength+cIdxOff; i++) { dL += adWeight[i]*fabs(adY[i]-adOffset[i]-adF[i]); dW += adWeight[i]; } } return dL/dW; }

void Master::treeInterfaceNodeBounds(int id, double lb, double ub) { if (VbcLog_ == NoVbc) return; //char string[256]; ostringstream info; if (isInfinity(fabs(lb))) { if (isInfinity(fabs(ub))) info << "I " << id << " \\iLower Bound: ---\\nUpper Bound: ---\\i"; else info << "I " << id << " \\iLower Bound: ---\\nUpper Bound: " << std::ios::fixed << std::setprecision(2) << std::setw(6) << ub << "\\i"; } else { if (isInfinity(fabs(ub))) info << "I " << id << " \\iLower Bound: " << std::ios::fixed << std::setprecision(2) << std::setw(6) << lb << "\\nUpper Bound: ---\\i"; else info << "I " << id << " \\iLower Bound: " << std::ios::fixed << std::setprecision(2) << std::setw(6) << lb << "\\nUpper Bound: " << std::ios::fixed << std::setprecision(2) << std::setw(6) << ub << "\\i"; } writeTreeInterface(info.str()); }

/** * Create an ObitFArray containing a unit area Gaussian in the center * \param inImage ObitImage giving the geometry of the output array * \param Beam Gaussian major, minor axis, position angle all in deg. * If there is no pixel spacing information in the inImage * descriptor then the size is assumed in pixels. * \return new ObitFArray, should be Unreffed when done */ ObitFArray* ObitConvUtilGaus (ObitImage *inImage, ofloat Beam[3]) { ObitFArray* outArray=NULL; ofloat amp, Cen[2], GauMod[3]; olong ndim=2, naxis[2]; /* Use Gaussian - make array from Input */ naxis[0] = inImage->myDesc->inaxes[0]; naxis[1] = inImage->myDesc->inaxes[1]; outArray = ObitFArrayCreate("Convolving function", ndim, naxis); /* Zero array */ ObitFArrayFill (outArray, 0.0); /* Insert Gaussian */ Cen[0] = (ofloat)(naxis[0]/2); /* zero ref */ Cen[1] = (ofloat)(naxis[1]/2); if (fabs(inImage->myDesc->cdelt[0])>0.0) { GauMod[0] = Beam[0] / fabs(inImage->myDesc->cdelt[0]); GauMod[1] = Beam[1] / fabs(inImage->myDesc->cdelt[0]); } else { /* Beam in pixels */ GauMod[0] = Beam[0]; GauMod[1] = Beam[1]; } GauMod[2] = Beam[2] - 90.0; /* Want Gaussian with normalized area */ amp = 1.0 / (2.0 * G_PI * (GauMod[0]/2.3548) * (GauMod[1]/2.3548)); /* Make elliptical Gaussian in outArray */ ObitFArray2DEGauss (outArray, amp, Cen, GauMod); return outArray; } /* end ObitConvUtilGaus */

/** * Calculates determinant in floating point number * @param 3x3 matrix * @return positive or not of determinant */ int ConvexHull::determ(const double (*m)[3]) { double h[] = { m[0][0] * m[1][1] * m[2][2], m[1][0] * m[2][1] * m[0][2], m[2][0] * m[0][1] * m[1][2], m[2][0] * m[1][1] * m[0][2], m[1][0] * m[0][1] * m[2][2], m[0][0] * m[2][1] * m[1][2] }; double hmax = fabs(h[0]); for (int i = 1; i < 6; i++) { double ah = fabs(h[i]); if (hmax < ah) { hmax = ah; } } hmax = (hmax > 1.0 ? hmax : 1.0); double vthr = PREC * hmax; double vdet = h[0] + h[1] + h[2] - h[3] - h[4] - h[5]; int is = 0; if (fabs(vdet) < vthr) { is = 0; } else if (vdet > 0.0) { is = 1; } else { is = -1; } return is; }

// Computes an estimate of the line spacing of the block from the median // of the spacings between adjacent overlapping textlines. void BaselineBlock::EstimateLineSpacing() { GenericVector<float> spacings; for (int r = 0; r < rows_.size(); ++r) { BaselineRow* row = rows_[r]; // Exclude silly lines. if (fabs(row->BaselineAngle()) > M_PI * 0.25) continue; // Find the first row after row that overlaps it significantly. const TBOX& row_box = row->bounding_box(); int r2; for (r2 = r + 1; r2 < rows_.size() && !row_box.major_x_overlap(rows_[r2]->bounding_box()); ++r2); if (r2 < rows_.size()) { BaselineRow* row2 = rows_[r2]; // Exclude silly lines. if (fabs(row2->BaselineAngle()) > M_PI * 0.25) continue; float spacing = row->SpaceBetween(*row2); spacings.push_back(spacing); } } // If we have at least one value, use it, otherwise leave the previous // value unchanged. if (!spacings.empty()) { line_spacing_ = spacings[spacings.choose_nth_item(spacings.size() / 2)]; if (debug_level_ > 1) tprintf("Estimate of linespacing = %g\n", line_spacing_); } }