Example #1
0
static void average_float(t_average *x, t_floatarg f)
{
	int i, j = 0;
	t_float tendency;
	t_float geo = 1.0;

	x->x_average = 0;
		/* put value into array */
	x->x_input[x->x_inpointer] = f;
		/* calulate average */
	for(i = 0; i < x->x_index; i++)
	{
		if(x->x_mode == 0)	/* linear */
		{
			x->x_average += x->x_input[i] * (1.0 / (float)x->x_index);
		}
		else if(x->x_mode == 1)	/* geometric */
		{
			if(x->x_input[i] == 0)x->x_input[i] = 0.001;	/* need to cheat a bit... */
			geo *= x->x_input[i];
			if(i == x->x_index - 1)
				x->x_average = pow(geo, (1.0/(float)x->x_index));
		}
		else if(x->x_mode == 2)	/* weighted */
		{
			x->x_average += x->x_input[(j + x->x_inpointer + x->x_index) % x->x_index] * (float)(x->x_index - (i + 1));
			j--;	/* go back in array */
				/* normalise output */
			if(i == x->x_index - 1)
				x->x_average = x->x_average / (float)normalise(x->x_index - 1);
		} else post("average: internal error!");
	}
	if(++x->x_inpointer > x->x_index)
	{
		x->x_inpointer = 0;
		if(x->x_lastaverage < x->x_average)
		{
			tendency = 1;	/* getting more */
		}
		else if(x->x_lastaverage > x->x_average)
		{
			tendency = -1;	/* getting less */
		}
		else tendency = 0;	/* nothing has changed */
		outlet_float(x->x_outtendency, tendency);
		x->x_lastaverage = x->x_average;
	}
	outlet_float(x->x_outfloat, x->x_average);
}
Example #2
0
void uade_effect_run(struct uade_effect *ue, int16_t * samples, int frames)
{
    if (ue->enabled & (1 << UADE_EFFECT_ALLOW)) {
        if( ue->enabled & (1 << UADE_EFFECT_NORMALISE))
            normalise(1, samples, frames);
        if (ue->enabled & (1 << UADE_EFFECT_PAN))
            pan(ue->pan, samples, frames);
        if (ue->enabled & (1 << UADE_EFFECT_HEADPHONES))
            headphones(samples, frames);
        if (ue->enabled & (1 << UADE_EFFECT_HEADPHONES2) && ue->rate)
            headphones2(samples, frames);
        if (ue->enabled & (1 << UADE_EFFECT_GAIN))
            gain(ue->gain, samples, frames);
    }
}
Example #3
0
void upperbound(int* val)
{
	// Lowerbound and increment the last valid one
	for (int i = 0; i < 6; ++i)
		if (val[i] == -1)
			val[i] = (i == 1 || i == 2) ? 1 : 0;
		else if (i != 5 && val[i+1] != -1)
			;
		else
			++val[i];
	// Then decrement the second, to get an inclusive range
	--val[5];
	// Normalise the result
	normalise(val);
}
Example #4
0
RangePrivate::RangePrivate(int &row1, int &column1, bool rowStatic1, bool colStatic1,
                           int &row2, int &column2, bool rowStatic2, bool colStatic2, Range *parent) :
    q_ptr(parent),
    mTop(row1),
    mBottom(row2),
    mLeft(column1),
    mRight(column2),
    bRowStatic1(rowStatic1),
    bRowStatic2(rowStatic2),
    bColStatic1(colStatic1),
    bColStatic2(colStatic2) {

    normalise();

}
Example #5
0
/* find a target in the tree -- not quick! */
virt_dirent_t *
virtdir_find_tgt(virtdir_t *tp, const char *tgt, size_t tgtlen)
{
	/* we don't need no stinking binary searches */
	char	path[MAXPATHLEN];
	int	i;

	(void) normalise(tgt, tgtlen, path, sizeof(path));
	for (i = 0 ; i < tp->c ; i++) {
		if (tp->v[i].tgt && strcmp(tp->v[i].tgt, path) == 0) {
			return &tp->v[i];
		}
	}
	return NULL;
}
	Quaternion& Quaternion::rotationFromTo(const Vector3& from, const Vector3& to)
	{
		// Based on Stan Melax's article in Game Programming Gems
		// Copy, since cannot modify local
		Vector3 v0 = from;
		Vector3 v1 = to;
		v0.normalize();
		v1.normalize();

		const FLOAT32 d = v0.dotProduct(v1);
		if (d >= 1.0f) // If dot == 1, vectors are the same
		{
			return makeIdentity();
		}
		else if (d <= -1.0f) // exactly opposite
		{
			Vector3 axis(1.0f, 0.f, 0.f);
			axis = axis.crossProduct(v0);
			if (axis.length() == 0)
			{
				axis.set(0.f, 1.f, 0.f);
				axis = axis.crossProduct(v0);
			}
			// same as fromAngleAxis(PI, axis).normalize();
			set(axis.x, axis.y, axis.z, 0);
			normalise();
			return *this;
		}

		const FLOAT32 s = sqrtf((1 + d) * 2); // optimize inv_sqrt
		const FLOAT32 invs = 1.f / s;
		const Vector3 c = v0.crossProduct(v1)*invs;
		set(c.x, c.y, c.z, s * 0.5f);
		normalise();
		return *this;
	}
Example #7
0
/**
 * im_histplot:
 * @in: input image
 * @out: output image
 *
 * Plot a 1 by any or any by 1 image file as a max by any or 
 * any by max image using these rules:
 * 
 * <emphasis>unsigned char</emphasis> max is always 256 
 *
 * <emphasis>other unsigned integer types</emphasis> output 0 - maxium 
 * value of @in.
 *
 * <emphasis>signed int types</emphasis> min moved to 0, max moved to max + min.
 *
 * <emphasis>float types</emphasis> min moved to 0, max moved to any 
 * (square output)
 *
 * See also: im_hist_indexed(), im_histeq().
 *
 * Returns: 0 on success, -1 on error
 */
int 
im_histplot( IMAGE *in, IMAGE *out )
{
	IMAGE *t1;

	if( im_check_hist( "im_histplot", in ) )
		return( -1 );

	if( !(t1 = im_open_local( out, "im_histplot:1", "p" )) ||
		normalise( in, t1 ) ||
		plot( t1, out ) )
		return( -1 );
	
	return( 0 );
}
Example #8
0
vec3 colourHash(const std::string& str) {
    int hash = stringHash(str);

    if(hash == 0) hash++;

    int r = (hash/7) % 255;
    if(r<0) r=0;
    int g = (hash/3) % 255;
    if(g<0) g=0;
    int b = hash % 255;

    vec3 colour = normalise(vec3(r, g, b));

    return colour;
}
Example #9
0
/*!
  Sets this quaternion by converting the rotation from angle-axis formate.
  The angle is specified in radians.
*/
void
Quaternion::set(const double& angle, const vec3& axis)
{
    // assert:  axis[] is unit length
    //
	// The quaternion representing the rotation is
	//   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)

    double halfAngle = 0.5*angle;
    double sn = sin(halfAngle);
    w = cos(halfAngle);
    x = sn*axis.x();
    y = sn*axis.y();
    z = sn*axis.z();
    normalise();
}
void instrument_counterexamples(refactor_programt &prog,
    refactor_counterexamplest ces)
{
  symbol_tablet &st=prog.st;
  goto_functionst &gf=prog.gf;
  const refactor_programt::counterexample_locationst &ce_locs=
      prog.counterexample_locations;
  const goto_programt::targetst unified(unify(ce_locs));
  const std::set<irep_idt> all_keys(get_all_keys(ce_locs));
  const zero_valuest zeroes(get_zero_values(st, unified));
  normalise(all_keys, zeroes, ces);
  create_ce_arrays(st, gf, ces);
  create_ce_array_indexes(all_keys, st, gf);
  add_ce_goto(st, gf, ces.size());
  assign_ce_values(st, gf, ce_locs);
}
double ContactEnergy::energy() const
{
  /*
  this is if we don't want to use pre-specified contacts, but rather the closest points between each link
  and the object all iterations. Not necessarily needed for contact energy, but useful for GQ energy
  */
  if (mContactType == CONTACT_LIVE && mType != ENERGY_AUTOGRASP_QUALITY && mType != ENERGY_STRICT_AUTOGRASP)
  {
    mHand->getWorld()->findVirtualContacts(mHand, mObject);
    DBGP("Live contacts computation");
  }

  mHand->getGrasp()->collectVirtualContacts();

  //DBGP("Contact energy computation")
  //average error per contact
  VirtualContact *contact;
  vec3 p, n, cn;
  double totalError = 0;
  for (int i = 0; i < mHand->getGrasp()->getNumContacts(); i++)
  {
    contact = (VirtualContact *)mHand->getGrasp()->getContact(i);
    contact->getObjectDistanceAndNormal(mObject, &p, NULL);
    double dist = p.len();

    //this should never happen anymore since we're never inside the object
    //if ( (-1.0 * p) % n < 0) dist = -dist;

    //BEST WORKING VERSION, strangely enough
    totalError += fabs(dist);

    //let's try this some more
    //totalError += distanceFunction(dist);
    //cn = -1.0 * contact->getWorldNormal();

    //new version
    cn = contact->getWorldNormal();
    n = normalise(p);
    double d = 1 - cn % n;
    totalError += d * 100.0 / 2.0;
  }

  totalError /= mHand->getGrasp()->getNumContacts();

  //DBGP("Contact energy: " << totalError);
  return totalError;
}
Example #12
0
int specchord2(dataptr dz)
{
	int exit_status;
	int newvc, cc, vc, n;
	double pre_amptotal, post_amptotal;
	double thisamp, thisfrq;
	double hifrq = dz->param[CHORD_HIFRQ];
	double lofrq = dz->param[CHORD_LOFRQ];
	if(dz->brksize[CHORD_HIFRQ] || dz->brksize[CHORD_LOFRQ]) {
		if(hifrq < lofrq)
			swap(&hifrq,&lofrq);
	}
	if((exit_status = get_totalamp(&pre_amptotal,dz->flbufptr[0],dz->wanted))<0)
		return(exit_status);
	for(vc = 0; vc < dz->wanted; vc += 2)
		dz->windowbuf[0][AMPP] = 0.0F;
	for( cc = 0 ,vc = 0; cc < dz->clength; cc++, vc += 2) {
		for(n=0;n<dz->itemcnt;n++) {	  /* for each note of chord */
			if((exit_status = gotnewfrq2(&newvc,vc,&thisamp,&thisfrq,n,dz))<0)
				return(exit_status);
			if(exit_status==FALSE)
				continue;
			switch(dz->vflag[CHORD_BODY]) {
			case(CHD_LESSFUL):
				if((exit_status = move_data_into_appropriate_channel(vc,newvc,(float)thisamp,(float)thisfrq,dz))<0)
					return(exit_status);
				break;
			case(CHD_NORMAL):  
				if((exit_status = move_data_into_some_appropriate_channel(newvc,(float)thisamp,(float)thisfrq,dz))<0)
					return(exit_status);
				break;
			default:
				sprintf(errstr,"Unknown case in dz->vflag[CHROD_BODY]: specchord()\n");
				return(PROGRAM_ERROR); 
			}
		}
	}
	for(vc = 0; vc < dz->wanted; vc+=2)
		dz->flbufptr[0][AMPP] = dz->windowbuf[0][AMPP];
	if(dz->vflag[CHORD_FBOT] || dz->vflag[CHORD_FTOP]) {
		if((exit_status = remove_unwanted_frq_areas(lofrq,hifrq,dz))<0)
			return(exit_status);
	}
	if((exit_status = get_totalamp(&post_amptotal,dz->flbufptr[0],dz->wanted))<0)
		return(exit_status);
	return normalise(pre_amptotal,post_amptotal,dz);
}
Example #13
0
void RDirNode::updateSplinePoint(float dt) {
    if(parent == 0) return;

    //update the spline point
    vec2 td = (parent->getPos() - pos) * 0.5f;

    vec2 mid = pos + td;// - td.perpendicular() * pos.normal();// * 10.0;

    vec2 delta = (mid - spos);

    //dont let spos get more than half the length of the distance behind
    if(glm::length2(delta) > glm::length2(td)) {
        spos += normalise(delta) * (glm::length(delta) - glm::length(td));
    }

    spos += delta * std::min(1.0f, dt * 2.0f);
}
Example #14
0
void interact_enemy_and_player(entity* e, rgba c) {
    if (player* pl = overlaps_player(e)) {
        if (pl->_dangerous) {
            fx_explosion(e->_pos, 200.0f, 40, rgba(0.0f, 1.0f), 9.0f, 20);
            fx_explosion(e->_pos, 300.0f, 10, c, 3.0f, 20);
            fx_explosion(e->_pos, 300.0f, 50, rgba(0.0f, 1.0f), 6.0f, 30);
            fx_explosion(e->_pos, 600.0f, 20, rgba(0.0f, 1.0f), 3.0f, 37);

            sound_play(sfx::UNIT_EXPLODE, 0.0f, -15.0f);

            vec2 d = normalise(pl->_pos - e->_pos);
            pl->_vel *= 0.5f;
            pl->_vel += d * 100.0f;
            destroy_entity(e);
        }
    }
}
Example #15
0
GT PFC::miller_loop(const G2& QQ,const G1& PP)
{ 
	GT z;
    int i,j,n,nb,nbw,nzs;
    ECn3 A,Q;
	ECn P;
	ZZn Px,Py;
	BOOL precomp;
    ZZn6 res;
	Big X=*x;

	P=PP.g; Q=QQ.g;
#ifdef MR_ECN3_PROJECTIVE
	Q.norm();
#endif
	precomp=FALSE;
	if (QQ.ptable!=NULL) precomp=TRUE;

	normalise(P);
	extract(P,Px,Py);

    Px+=Px;  // because x^6+2 is irreducible.. simplifies line function calculation
    Py+=Py; 

    res=1;  
    A=Q;    // reset A
    nb=bits(X);
	res.mark_as_miller();
	j=0;

    for (i=nb-2;i>=0;i--)
    {
		res*=res;
		if (precomp) res*=gp(QQ.ptable,j,Px,Py);
		else         res*=g(A,A,Px,Py);

		if (bit(X,i)==1)
		{
			if (precomp) res*=gp(QQ.ptable,j,Px,Py);
			else         res*=g(A,Q,Px,Py);
		}
    }

	z.g=res;
	return z;
}
Example #16
0
void upperbound(const int* src, int* dst)
{
	//cerr << "UBSTART " << tostring(src) << endl;
	// Lowerbound and increment the last valid one
	for (int i = 0; i < 6; ++i)
		if (src[i] == -1)
			dst[i] = (i == 1 || i == 2) ? 1 : 0;
		else if (i != 5 && src[i+1] != -1)
			dst[i] = src[i];
		else
			dst[i] = src[i] + 1;
	// Then decrement the second, to get an inclusive range
	--dst[5];
	//cerr << "UBNORM " << tostring(dst) << endl;
	// Normalise the result
	normalise(dst);
}
Example #17
0
vector<double> HMM::dhmm_em(field<rowvec> data, int max_iter) {
	/*
	 * Find the ML/MAP parameters of an HMM with discrete outputs using EM.
	 *
	 * Notation: Q(t) = hidden state, Y(t) = observation
	 */
	//cout << "dhmm_em"<<endl;

	double previous_loglik = - std::numeric_limits<double>::max();
	double thresh =  1e-4;
	double converged = 0;
	int num_iter = 1;
	int obs_prior_weight = 0; 	// weighting term for uniform dirichlet prior on expected emissions

	vector<double> LL;

	while ((num_iter <= max_iter) && !converged)  {

		double loglik = 0;
		int S = obsmat.n_rows;
		int O = obsmat.n_cols;

		mat exp_num_trans = zeros(S,S);		// all values initialized here for allocation issue (?) TODO: to be tested
		mat exp_num_visits1 = zeros(S,1);
		mat exp_num_visitsT = zeros(S,1);
		mat exp_num_emit = obs_prior_weight*ones(S,O);	// obs_prior_weight is dirichlet in Kevin Murphy's code

		// E step
		compute_ess_dhmm(prior, transmat, obsmat, data,
				loglik, exp_num_trans, exp_num_visits1, exp_num_emit, exp_num_visitsT);

		// M step
		prior = normalise(exp_num_visits1);
		if (!exp_num_trans.is_empty())
			transmat = mk_stochastic(exp_num_trans);
		obsmat = mk_stochastic(exp_num_emit);

		std::cout << "iteration " << num_iter << ", loglik = " << loglik << endl;

		num_iter =  num_iter + 1;
		converged = em_converged(loglik, previous_loglik, thresh);
		previous_loglik = loglik;
		LL.push_back(loglik);
	}
	return LL;
}
void FPSController::moveCamera(Ogre::Real dt)
{
	Ogre::Real pitchAngle, pitchAngleSign;

	// Yaw the camera according to the mouse relative movement.
	cameraYawNode->yaw(rotX);

	// Pitch the camera according to the mouse relative movement.
	cameraPitchNode->pitch(rotY);

	// Move the camera around the world.
	auto normDir = accel * velocity * dt;
	normDir.normalise();

	cameraNode->translate(cameraYawNode->getOrientation() * cameraPitchNode->getOrientation() * normDir, Ogre::Node::TS_LOCAL);

	// Angle of rotation around the X-Axis.
	pitchAngle = (2 * Ogre::Degree(Ogre::Math::ACos(cameraPitchNode->getOrientation().w)).valueDegrees());

	// Just to determine the sign of the angle we pick up above, the value
	// itself does not interest us.
	pitchAngleSign = cameraPitchNode->getOrientation().x;

	// Limit the pitch between -90 and +90 degrees, Quake3 style.
	if (pitchAngle > 90.0f) {
		if (pitchAngleSign > 0) {
			// Set orientation to 90 degrees on X-Axis.
			cameraPitchNode->setOrientation(Ogre::Quaternion(Ogre::Math::Sqrt(0.5f), Ogre::Math::Sqrt(0.5f), 0, 0));
		} else if (pitchAngleSign < 0) {
			// Set orientation to -90 degrees on X-Axis.
			cameraPitchNode->setOrientation(Ogre::Quaternion(Ogre::Math::Sqrt(0.5f), -Ogre::Math::Sqrt(0.5f), 0, 0));
		}
	} 

	// TODO: Terrain clamp

	// Notify cameraNode's children.
	cameraYawNode->needUpdate();
	cameraPitchNode->needUpdate();
	cameraRollNode->needUpdate();

	// Update the camera ray.
	camRay.setOrigin(cam->getRealPosition());
	camRay.setDirection(cam->getRealDirection());
}
Example #19
0
int inner_form_loop(int windows_in_buf,dataptr dz)
{
	int exit_status;
	int wc;
	double pre_amptotal, post_amptotal;
 	for(wc=0; wc<windows_in_buf; wc++) {
		if(dz->total_windows==0) {
			if((exit_status = move_along_formant_buffer(dz))!=CONTINUE)
				return(exit_status);
			dz->flbufptr[0] += dz->wanted;
			dz->total_windows++;
			dz->time = (float)(dz->time + dz->frametime);
			continue;			/* SKIP FIRST (ZERO-AMP) WINDOW */
		}
		if(dz->mode==FORM_REPLACE) {
			rectify_window(dz->flbufptr[0],dz);
			if((exit_status = extract_specenv(0,0,dz))<0)
				return(exit_status);
		}
		dz->specenvamp2  = dz->flbufptr[1];
		pre_amptotal = post_amptotal = 0.0;
		if((exit_status = get_totalamp(&pre_amptotal,dz->flbufptr[0],dz->wanted))<0)
			return(exit_status);
		if((exit_status = do_specform(dz))<0)
			return(exit_status);
		if((exit_status = form_filter(dz))<0)
				return(exit_status);
		if((exit_status = get_totalamp(&post_amptotal,dz->flbufptr[0],dz->wanted))<0)
			return(exit_status);
		if((exit_status = normalise(pre_amptotal,post_amptotal,dz))<0)
			return(exit_status);
		if(!flteq(dz->param[FORM_GAIN],1.0)) {
			if((exit_status = form_gain(dz))<0)
				return(exit_status);
  				/* NB This check must be BEFORE move_along_formant_buffer() */
		}
		dz->flbufptr[0] += dz->wanted;
  		if(++dz->total_windows >= dz->wlength)
			return(FINISHED);
		if((exit_status = move_along_formant_buffer(dz))!=CONTINUE)
			return(exit_status);
		dz->time = (float)(dz->time + dz->frametime);
	}
	return(CONTINUE);
}
Example #20
0
/* This function simulates a number of scans around the object, then writes the scans to files 
   along with all the grasps found
*/
void DBaseBatchPlanner::takeScans()
{
	ScanSimulator sim;

	sim.setOptics(-45.0 , 45.0 , 400 ,
		      -45.0 , 45.0 , 400 );
	sim.setType(ScanSimulator::WORLD_COORDINATES);

	int numAltitudes = 3;
	float altitudes[3] = {(float)M_PI/6.0f, (float)M_PI/3.0f, (float)M_PI/2.0f };
	int numSamples[3] = {8, 4, 1};

	float distance = 1000; //1 meter

	std::vector<position> cloud;
	std::vector<RawScanPoint> rawData;

	for (int i=0; i<numAltitudes; i++) {
		float theta = altitudes[i];
		float phi_step = M_PI / numSamples[i];

		for (int j=0; j<numSamples[i]; j++) {
			float phi = phi_step * j;
			
			vec3 loc( distance * cos(theta) * cos(phi), 
				  distance * cos(theta) * sin(phi),
				  distance * sin(theta) );
			//looking back towards the origin
			vec3 dir = -1 * normalise(loc);
			vec3 up(0,0,1);
			sim.setPosition(loc, dir, up, ScanSimulator::STEREO_CAMERA);

			cloud.clear();
			rawData.clear();

			sim.scan(&cloud, &rawData);

			writeCloudToFile(i, j, cloud);
			vec3 foo, trueUp;
			position foop;
			sim.getPosition(foop,foo,trueUp);
			writeRawToFile(i, j, rawData, loc, dir, trueUp);
		}
	}
}
/* takes mouse position on screen and return ray in world coords */
vec3 get_ray_from_mouse (float mouse_x, float mouse_y) {
    // screen space (viewport coordinates)
    float x = (2.0f * mouse_x) / g_gl_width - 1.0f;
    float y = 1.0f - (2.0f * mouse_y) / g_gl_height;
    float z = 1.0f;
    // normalised device space
    vec3 ray_nds = vec3 (x, y, z);
    // clip space
    vec4 ray_clip = vec4 (ray_nds.v[0], ray_nds.v[1], -1.0, 1.0);
    // eye space
    vec4 ray_eye = inverse (proj_mat) * ray_clip;
    ray_eye = vec4 (ray_eye.v[0], ray_eye.v[1], -1.0, 0.0);
    // world space
    vec3 ray_wor = vec3 (inverse (view_mat) * ray_eye);
    // don't forget to normalise the vector at some point
    ray_wor = normalise (ray_wor);
    return ray_wor;
}
Example #22
0
void calcnormal(CTriangle3d t, CPoint3d &normal)
{
	CPoint3d v1, v2;

   v1.x = t.aX - t.bX;
   v1.y = t.aY - t.bY;
   v1.z = t.aZ - t.bZ;

   v2.x = t.bX - t.cX;
   v2.y = t.bY - t.cY;
   v2.z = t.bZ - t.cZ;

   normal.x = v1.y * v2.z - v1.z * v2.y;
   normal.y = v1.z * v2.x - v1.x * v2.z;
   normal.z = v1.x * v2.y - v1.y * v2.x;

   normalise(normal);
}
void Board::newellSquare(GLdouble *vec1,GLdouble *vec2,GLdouble *vec3,GLdouble *vec4,GLdouble *normal)
{
  normal[0] = (vec1[1]-vec2[1])*(vec1[2]+vec2[2]) + 
	      (vec2[1]-vec3[1])*(vec2[2]+vec3[2]) + 
	      (vec3[1]-vec4[1])*(vec3[2]+vec4[2]) +
	      (vec4[1]-vec1[1])*(vec4[2]+vec1[2]);
  normal[1] = (vec1[2]-vec2[2])*(vec1[0]+vec2[0]) + 
	      (vec2[2]-vec3[2])*(vec2[0]+vec3[0]) + 
	      (vec3[2]-vec4[2])*(vec3[0]+vec4[0]) +
	      (vec4[2]-vec1[2])*(vec4[0]+vec1[0]);
  normal[2] = (vec1[0]-vec2[0])*(vec1[1]+vec2[1]) + 
	      (vec2[0]-vec3[0])*(vec2[1]+vec3[1]) + 
	      (vec3[0]-vec4[0])*(vec3[1]+vec4[1]) +
	      (vec4[0]-vec1[0])*(vec4[1]+vec1[1]);

  normalise(normal);

}
double PitchDetector::detectSdfPitchForBlock (float* samples, int numSamples)
{
    const int minSample = int (sampleRate / maxFrequency);
    const int maxSample = int (sampleRate / minFrequency);
    
    lowFilter.reset();
    highFilter.reset();
    lowFilter.processSamples (samples, numSamples);
    highFilter.processSamples (samples, numSamples);
    
    sdfAutocorrelate (samples, numSamples, buffer1.getData());
    normalise (buffer1.getData(), buffer1.getSize());
    
    // find first minimum that is below a threshold
    const float threshold = 0.25f;
    const float* sdfData = buffer1.getData();
    float min = 1.0f;
    int index = 0;
    
    for (int i = minSample; i < maxSample; ++i)
    {
        const float prevSample = sdfData[i - 1];
        const float sample =  sdfData[i];
        const float nextSample =  sdfData[i + 1];
        
        if (sample < prevSample
            && sample < nextSample
            && sample < threshold)
        {
            if (sample < min)
            {
                min = sample;
                index = i;
            }
//            return sampleRate / i;
//            break;
        }
    }
    
    if (index != 0)
        return sampleRate / index;
    
    return 0.0;
}
/*!
    \fn CvBinGabAdaFeatureSelect::train(int nfeatures)
 */
void CvBinGabAdaFeatureSelect::train(int nfeatures)
{
  double error = 0.0; 
  nselecfeatures = 0;
  nexpfeatures = nfeatures;
  char *signfilename = new char[30];
  strcpy(signfilename, "significant.txt");
  FILE * file = fopen(signfilename, "w");
  fclose( file );
  delete [] signfilename;
  
  for (int i = 0; i < nexpfeatures; i++)
  {
    current = i;
    if (old_pool->getSize() < 1) break;
    
    /* prepare log files */
    char *allfeaturefilename = new char[30];
    sprintf(allfeaturefilename, "allfeature_%d.txt", current);
    FILE * file1 = fopen(allfeaturefilename, "w");
    fclose( file1 );
    delete [] allfeaturefilename;
    
    char *disfeaturefilename = new char[30];
    sprintf(disfeaturefilename, "discard_%d.txt", current);
    FILE * file2 = fopen(disfeaturefilename, "w");
    fclose( file2 );
    delete [] disfeaturefilename;
    
    char *featurefilename = new char[30];
    sprintf(featurefilename, "feature_%d.txt", current); 
    FILE * file3 = fopen(featurefilename, "w");
    fclose( file3 );
    delete [] featurefilename;
    /* prepare log files */
    
    normalise();
    trainallfeatures();
    error = findminerror();
    update(); 
    saveweaks( weaksname.c_str() );
    nselecfeatures++;
  }
}
myVecteur2D::myVecteur2D(myVecteur2D* v1, myVecteur2D* v2){ // créer un rebond : premier vecteur sur le deuxième

	vertex originrebond;
	originrebond.x = 0;
	originrebond.y = 0;

	int sens;

	sens = intersectionDroites(v1, v2, originrebond);


	if (sens > 0)
	{
		v1->normalise();
		v2->normalise();
		//std::cout << "V1 : " << v1->getNorme() << "  v2 : " << v2->getNorme() << " v1x : " << v1->getxdir() << " v1y : " << v1->getydir() << " v2x : " << v2->getxdir() << " v2y : " << v2->getydir() << std::endl;

		vertex dirOrtho;
		dirOrtho.x =  v2->getydir();
		dirOrtho.y = - v2->getxdir();

		float psv12 = produitscalaire(v1,v2); //produit scalaire de v1 sur v2
		float psv12ortho = v1->getxdir() * dirOrtho.x + v1->getydir() * dirOrtho.y;



		xdir = psv12*v2->getxdir() - psv12ortho * dirOrtho.x;
		ydir = psv12*v2->getydir() - psv12ortho * dirOrtho.y;
		origin = originrebond;

		//std::cout << "psv12 : " << psv12 << "  ortho " << psv12ortho << "xdir : " << xdir << "direct " << psv12*v2->getxdir() + psv12ortho * dirOrtho.x<< std::endl;

		normalise();

	}
	else
	{
//on ne crée rien
	}


}
Example #27
0
void
View::lookAt(double eyex, double eyey, double eyez, 
	     double centerx, double centery, double centerz, double twist)
{
    Vec3 eye(eyex, eyey, eyez);
    Vec3 negz(0., 0., -1.); 
    Vec3 dir;
    Quaternion q_dir, q_twist;

    center = Vec3(centerx, centery, centerz);
    dir = center-eye;
    zoom = len(dir);
    normalise(dir);

    q_dir = quatFromUV(negz, dir);
    q_twist = quatFromAxisRot(negz, twist * M_PI / 180);
    orient = conjugate(q_dir*q_twist);		// quaternion multiplication

    glComputeModelview();
}
Example #28
0
/*!
  Sets this quaternion by converting from rotation matrix \a R.
*/
void
Quaternion::set(const mat3 &R)
{
    // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
    // article "Quaternion Calculus and Fast Animation".

    double trace = R.element(0,0)+R.element(1,1)+R.element(2,2);
    double root;

    if ( trace > 0.0 )
    {
        // |w| > 1/2, may as well choose w > 1/2
        root = sqrt(trace+1.0);  // 2w
        w = 0.5*root;
        root = 0.5/root;  // 1/(4w)
        x = (R.element(1,2)-R.element(2,1))*root;
        y = (R.element(2,0)-R.element(0,2))*root;
        z = (R.element(0,1)-R.element(1,0))*root;
    }
    else
    {
        // |w| <= 1/2
        static int next[3] = { 1, 2, 0 };
        int i = 0;
        if ( R.element(1,1) > R.element(0,0) )
            i = 1;
        if ( R.element(2,2) > R.element(i,i) )
            i = 2;
        int j = next[i];
        int k = next[j];

        root = sqrt(R.element(i,i)-R.element(j,j)-R.element(k,k)+1.0);
        double* quat[3] = { &x, &y, &z };
        *quat[i] = 0.5*root;
        root = 0.5/root;
        w = (R.element(j,k)-R.element(k,j))*root;
        *quat[j] = (R.element(i,j)+R.element(j,i))*root;
        *quat[k] = (R.element(i,k)+R.element(k,i))*root;
    }
    normalise();
}
Example #29
0
void AscentAP::SetLaunchAzimuth (double azimuth)
{
	launch_azimuth = azimuth;

	// current launch location in local planet frame
	VECTOR3 pos, equ, dir, nml, ne, nd;
	double lng, lat, rad;
	double slng, clng, slat, clat;
	double saz = sin(azimuth), caz = cos(azimuth);
	OBJHANDLE hRef = vessel->GetGravityRef();
	vessel->GetGlobalPos(pos);
	oapiGlobalToLocal (hRef, &pos, &equ);
	oapiLocalToEqu (hRef, equ, &lng, &lat, &rad);
	slng = sin(lng), clng = cos(lng), slat = sin(lat), clat = cos(lat);
	normalise(equ); // unit radius vector
	
	// launch direction in local planet frame
	dir = _V(-clng*slat*caz - slng*saz, clat*caz, -slng*slat*caz + clng*saz);

	// normal of orbital plane in local planet frame
	nml = crossp(dir, equ);

	// normal of equator plane in local planet frame
	ne = _V(0,1,0);

	// direction of ascending node
	nd = unit (crossp(nml, ne));

	// orbit inclination
	tgt.inc = acos(dotp(nml, ne));

	// longitude of ascending node
	tgt.lan = atan2(nd.z, nd.x);

	// rotation matrix from equator plane to target orbit plane
	double sinc = sin(tgt.inc), cinc = cos(tgt.inc);
	double slan = sin(tgt.lan), clan = cos(tgt.lan);
	MATRIX3 R1 = _M(1,0,0, 0,cinc,sinc, 0,-sinc,cinc);
	MATRIX3 R2 = _M(clan,0,-slan, 0,1,0, slan,0,clan);
	tgt.R = mul(R2,R1);
}
Example #30
0
//build a quaternion from 3 euler angles
Quaternion::Quaternion(double zdeg, double ydeg, double xdeg){
	//if reverse order, change zdeg to xdeg and xdeg to zdeg
	//3-2-1 
	mQuatVal[0] = (cos(xdeg/2.0) * cos(ydeg/2.0) * cos(zdeg/2.0)) + (sin(xdeg/2.0) * sin(ydeg/2.0) * sin(zdeg/2.0));
	mQuatVal[1] = (sin(xdeg/2.0) * cos(ydeg/2.0) * cos(zdeg/2.0)) - (cos(xdeg/2.0) * sin(ydeg/2.0) * sin(zdeg/2.0));
	mQuatVal[2] = (cos(xdeg/2.0) * sin(ydeg/2.0) * cos(zdeg/2.0)) + (sin(xdeg/2.0) * cos(ydeg/2.0) * sin(zdeg/2.0));
	mQuatVal[3] = (cos(xdeg/2.0) * cos(ydeg/2.0) * sin(zdeg/2.0)) - (sin(xdeg/2.0) * sin(ydeg/2.0) * cos(zdeg/2.0));

	normalise(mQuatVal);
	mW = mQuatVal[0];
	mVec = Vec3d(mQuatVal[1],mQuatVal[2],mQuatVal[3]);
	//angle always must be accurate - used for deriv
	mAngle = 2.0 * acos(mQuatVal[0]);
	double sinRes =  sin(mAngle * .5);
	if(sinRes != 0 ) {//means theta is 0 or a multiple of 0 - only way theta/2 will have sin 0, so no rotation
		mAxis = mVec * (1.0 / sin(mAngle * .5));
	} else {//if theta == 0 then no rotation, so doesn't matter
		mAxis = Vec3d(vl_0);
		mAxis[0] = 1;		//arbitrary rotation - no rot needed if angle ==0
	}
}//