Example #1
0
//最大熵和法
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;
}
Example #2
0
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--]));
	}
}
Example #4
0
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 );
}
Example #5
0
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;
}
Example #6
0
File: smath.C Project: zear/sabre
/*
 *	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);
}
Example #7
0
 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]);
     }
   }
 }
Example #8
0
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);
    }
}
Example #9
0
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;
}
Example #11
0
/*! 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;
}
Example #12
0
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;
}
Example #13
0
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;
}
Example #14
0
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;
}
Example #15
0
/**
 * 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);
		}
	}
}
Example #19
0
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);
}
Example #20
0
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);
  }
}
Example #21
0
/*
 * 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
}
Example #22
0
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;
    }
}
Example #23
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;
}
Example #24
0
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;
}
Example #25
0
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);
}
Example #26
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;
}
Example #27
0
File: master.cpp Project: ogdf/ogdf
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());
}
Example #28
0
/**
 * 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 */
Example #29
0
/**
 * 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;
}
Example #30
0
// 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_);
  }
}