Esempio n. 1
0
fcomplex
cexpf(fcomplex z) {
	fcomplex	ans;
	float		x, y, c, s;
	double		t;
	int		n, ix, iy, hx, hy;

	x = F_RE(z);
	y = F_IM(z);
	hx = THE_WORD(x);
	hy = THE_WORD(y);
	ix = hx & 0x7fffffff;
	iy = hy & 0x7fffffff;
	if (iy == 0) {		/* y = 0 */
		F_RE(ans) = expf(x);
		F_IM(ans) = y;
	} else if (ix == 0x7f800000) {	/* x is +-inf */
		if (hx < 0) {
			if (iy >= 0x7f800000) {
				F_RE(ans) = zero;
				F_IM(ans) = zero;
			} else {
				sincosf(y, &s, &c);
				F_RE(ans) = zero * c;
				F_IM(ans) = zero * s;
			}
		} else {
			if (iy >= 0x7f800000) {
				F_RE(ans) = x;
				F_IM(ans) = y - y;
			} else {
				sincosf(y, &s, &c);
				F_RE(ans) = x * c;
				F_IM(ans) = x * s;
			}
		}
	} else {
		sincosf(y, &s, &c);
		if (ix >= 0x42B171AA) {	/* |x| > 88.722... ~ log(2**128) */
#if defined(__i386) && !defined(__amd64)
			int	rp = __swapRP(fp_extended);
#endif
			t = __k_cexp(x, &n);
			F_RE(ans) = (float)scalbn(t * (double)c, n);
			F_IM(ans) = (float)scalbn(t * (double)s, n);
#if defined(__i386) && !defined(__amd64)
			if (rp != fp_extended)
				(void) __swapRP(rp);
#endif
		} else {
			t = expf(x);
			F_RE(ans) = t * c;
			F_IM(ans) = t * s;
		}
	}
	return (ans);
}
Esempio n. 2
0
void cooler::update() noexcept
{
    if(time <= 0)
    {
        alive = 1;
        if(pos == targetPos)
        {
            targetPos = vec2f(rand()%(arenaX*40), rand()%(arenaY*40));
            vec2f c = targetPos - pos;
            float angle = atan2(c.x, c.y);
            sincosf(angle, &velocity.x, &velocity.y);
            velocity *= speed;
        }
        vec2f tpos = targetPos - pos;
        if(tpos < velocity*window::deltaTime)
        {
            pos = targetPos;
        }else
        pos += velocity*window::deltaTime;
        resources::coolEffect.pos = pos;
    }else
    {
        alive = 0;
        time -= window::deltaTime;
        if(time <= 0)
            resources::coolEffect.start();
    }
    resources::coolEffect.update();
}
Esempio n. 3
0
void Java_com_example_sema4_ACARSActivity_initRTL(JNIEnv * env, jobject obj){
	float AMFreq;
	int ind;
	channel_t *initial = &global;

	unsigned int Fd[4];
	Fd[0] =((int)(1000000 * atof("136.872") + RTLRATE / 2) / RTLRATE) * RTLRATE;
	unsigned int Fc =  136872000;//chooseFc(Fd, 1);
	initial->chn = 0;
	initial->inmode = 3;
	initial->Fr = (float) Fd[0];
	initial->Infs = RTLRATE;
	initial->cwf = malloc(RTLMULT * sizeof(float));
	initial->swf = malloc(RTLMULT * sizeof(float));
	AMFreq = (initial->Fr - (float)Fc) / (float)(RTLINRATE) * 2.0 * M_PI;

			for (ind = 0; ind < RTLMULT; ind++) {
				sincosf(AMFreq * ind, &(initial->swf[ind]), &(initial->cwf[ind]));
				initial->swf[ind] /= RTLMULT;
				initial->cwf[ind] /= RTLMULT;
			}

	initMsk(&global);
	initAcars(&global);
}
Esempio n. 4
0
void structfac(int na, int nr, TYPE *a, TYPE *h, TYPE *E) {
    int i, j;
    TYPE A, B, twopi;
    twopi = 6.28318584f;

    TYPE f2 = 0.0f;

    for (i = 0; i < na; i++)
        f2 += a[DIM2_A * i] * a[DIM2_A * i];

    f2 = 1.0f / sqrtf(f2);

    int NH = DIM2_H * nr;
    int NA = DIM2_A * na;
    int NE = DIM2_E * nr;

    for (i = 0; i < nr; i++) {
        A = 0.0f;
        B = 0.0f;

        TYPE h_arg1 = h[DIM2_H * i + 0];
        TYPE h_arg2 = h[DIM2_H * i + 1];
        TYPE h_arg3 = h[DIM2_H * i + 2];

        for (j = 0; j < na; j++) {
            TYPE A1, B1;
            TYPE arg = twopi * (h_arg1 * a[DIM2_A * j + 1] + h_arg2 * a[DIM2_A * j + 2] + h_arg3 * a[DIM2_A * j + 3]);
            sincosf(arg, &B1, &A1);
            A += a[DIM2_A * j] * A1;
            B += a[DIM2_A * j] * B1;
        }
        E[DIM2_E * i] = A * f2;
        E[DIM2_E * i + 1] = B * f2;
    }
}
Esempio n. 5
0
int main(void)
{
	#pragma STDC FENV_ACCESS ON
	float ysin, ycos;
	float dsin, dcos;
	int e, i, err = 0;
	struct f_ff *p;

	for (i = 0; i < sizeof t/sizeof *t; i++) {
		p = t + i;

		if (p->r < 0)
			continue;
		fesetround(p->r);
		feclearexcept(FE_ALL_EXCEPT);
		sincosf(p->x, &ysin, &ycos);
		e = fetestexcept(INEXACT|INVALID|DIVBYZERO|UNDERFLOW|OVERFLOW);

		if (!checkexcept(e, p->e, p->r)) {
			printf("%s:%d: bad fp exception: %s sincosf(%a)=%a,%a, want %s",
				p->file, p->line, rstr(p->r), p->x, p->y, p->y2, estr(p->e));
			printf(" got %s\n", estr(e));
			err++;
		}
		dsin = ulperr(ysin, p->y, p->dy);
		dcos = ulperr(ycos, p->y2, p->dy2);
		if (!checkulp(dsin, p->r) || !checkulp(dcos, p->r)) {
			printf("%s:%d: %s sincosf(%a) want %a,%a got %a,%a, ulperr %.3f = %a + %a, %.3f = %a + %a\n",
				p->file, p->line, rstr(p->r), p->x, p->y, p->y2, ysin, ycos,
				dsin, dsin-p->dy, p->dy, dcos, dcos-p->dy2, p->dy2);
			err++;
		}
	}
	return !!err;
}
Esempio n. 6
0
/* //////////////////////////////////////////////////////////////////////////////////////
 * implementation
 */
tb_void_t tb_sincosf(tb_float_t x, tb_float_t* s, tb_float_t* c)
{
#ifdef TB_CONFIG_LIBM_HAVE_SINCOSF
    sincosf(x, s, c);
#else
    if (s) *s = sinf(x);
    if (c) *c = cosf(x);
#endif
}
Esempio n. 7
0
QLA_F_Complex
QLA_F_cexpi(QLA_F_Real theta)
{
  QLA_F_Real s, c;
  sincosf(theta, &s, &c);
  QLA_F_Complex z;
  QLA_c_eq_r_plus_ir(z, c, s);
  return z;
}
Esempio n. 8
0
void LocalizationModule::update()
{
    // Modify based on control portal
    if (lastReset != resetInput.message().timestamp())
    {
        lastReset = resetInput.message().timestamp();
        particleFilter->resetLocTo(resetInput.message().x(),
                                   resetInput.message().y(),
                                   resetInput.message().h());
    }

    // Calculate the deltaX,Y,H (PF takes increments from robot frame)
    lastOdometry.set_x(curOdometry.x());
    lastOdometry.set_y(curOdometry.y());
    lastOdometry.set_h(curOdometry.h());

    // Want odometry to give information relative to current robot frame
    // IE choose to have robot frame change as the robot moves
    float sinH, cosH;
    sincosf(motionInput.message().h(), &sinH, &cosH);
    float rotatedX =   cosH*motionInput.message().x()
                     + sinH*motionInput.message().y();
    float rotatedY =   cosH*motionInput.message().y()
                     - sinH*motionInput.message().x();

    curOdometry.set_x(rotatedX * ODOMETRY_X_FRICTION_FACTOR);
    curOdometry.set_y(rotatedY * ODOMETRY_Y_FRICTION_FACTOR);
    curOdometry.set_h(motionInput.message().h() * ODOMETRY_HEADING_FRICTION_FACTOR);

    deltaOdometry.set_x(curOdometry.x() - lastOdometry.x());
    deltaOdometry.set_y(curOdometry.y() - lastOdometry.y());
    deltaOdometry.set_h(curOdometry.h() - lastOdometry.h());

    // Ensure deltaOdometry is reasonable (initial fix lost in git?)
    if((fabs(deltaOdometry.x()) > 3.f) || (fabs(deltaOdometry.y()) > 3.f))
    {
        deltaOdometry.set_x(0.f);
        deltaOdometry.set_y(0.f);
        deltaOdometry.set_h(0.f);
    }

    // Update the Particle Filter with the new observations/odometry
    particleFilter->update(deltaOdometry, visionInput.message());

    // Update the locMessage and the swarm (if logging)
    portals::Message<messages::RobotLocation> locMessage(&particleFilter->
                                                         getCurrentEstimate());

#ifdef LOG_LOCALIZATION
    portals::Message<messages::ParticleSwarm> swarmMessage(&particleFilter->
                                                           getCurrentSwarm());
    particleOutput.setMessage(swarmMessage);
#endif

    output.setMessage(locMessage);
}
Esempio n. 9
0
File: geom.c Progetto: jsgf/terrain
void quat_axis_angle(quat_t *q, const vec3_t *v, float angle)
{
	float s, c;
	sincosf(angle/2, &s, &c);

	q->w = c;
	q->v = *v;
	vec3_normalize(&q->v);
	vec3_scale(&q->v, s);
}
Esempio n. 10
0
QPoint FieldViewerPainter::getRelLoc(messages::RobotLocation loc, float dist, float bear)
{
    float sin, cos;
    float ninetyDeg = 1.5707963;
    sincosf((loc.h() + bear), &sin, &cos);

    float relX = dist*cos + loc.x();
    float relY = dist*sin + loc.y();
    QPoint relLoc(relX,relY);
    return relLoc;
}
Esempio n. 11
0
    Color3f sample(BSDFQueryRecord &bRec, const Point2f &sample_) const {
        Point2f sample(sample_);
        if (Frame::cosTheta(bRec.wi) <= 0)
            return Color3f(0.0f);

        bRec.measure = ESolidAngle;

        // 1. Select diffuse or specular
        bool useSpecular = true;
        if (sample.x() <= m_specSamplingWeight) {
            sample.x() /= m_specSamplingWeight;
        } else {
            sample.x() = (sample.x() - m_specSamplingWeight) / m_diffSamplingWeight;
            useSpecular = false;
        }

        if (useSpecular) {
            // this is a tricky one
            // See http://mathinfo.univ-reims.fr/IMG/pdf/Using_the_modified_Phong_reflectance_model_for_Physically_based_rendering_-_Lafortune.pdf
            
            float sinTheta = std::sqrt(1.0f - std::pow(sample.y(), 2.0f/(m_exp + 1.0f)));
            float cosTheta = std::pow(sample.y(), 1.0f/(m_exp + 1.0f));
            float phi = 2.0f * M_PI * sample.x();
            float cosPhi, sinPhi;
            sincosf(phi, &sinPhi, &cosPhi);
            // direction from the lobe axis (i.e. reflection of wi) in lobe coordinate
            Vector3f lobeAxis = reflect(bRec.wi);
            Vector3f lobeWo(
                    sinTheta * std::cos(phi),
                    sinTheta * std::sin(phi),
                    cosTheta
            );
            bRec.wo = Frame(lobeAxis).toWorld(lobeWo);
            
            // check that we are on the hemisphere
            if(Frame::cosTheta(bRec.wo) <= 0.0f)
                return Color3f(0.0f);
        } else {
            /* Warp a uniformly distributed sample on [0,1]^2
                to a direction on a cosine-weighted hemisphere */
            bRec.wo = squareToCosineHemisphere(sample);
        }

        /* Relative index of refraction: no change */
        bRec.eta = 1.0f;

        // the importance-weighted sample is given by
        //      f / pdf * cos(wo)
        Color3f f_eval = eval(bRec);
        float pdf_eval = pdf(bRec);
        if(pdf_eval <= 0.0f) return Color3f(0.0f); // let's not explode here
        
        return f_eval / pdf_eval * Frame::cosTheta(bRec.wo);
    }
Esempio n. 12
0
void                P3DMath::SinCosf  (float              *sina,
                                       float              *cosa,
                                       float               a)
 {
  #ifdef HAVE_SINCOSF
  sincosf(a,sina,cosa);
  #else
  *sina = sinf(a);
  *cosa = cosf(a);
  #endif
 }
Esempio n. 13
0
void angles_to_vectors (const float *angles, float *forward, float *right, float *up) {
  float ps, pc;
  float ys, yc;
  float rs, rc;

  sincosf(deg2rad(angles[ANGLE_PITCH]), &ps, &pc);
  sincosf(deg2rad(angles[ANGLE_YAW]),   &ys, &yc);
  sincosf(deg2rad(angles[ANGLE_ROLL]),  &rs, &rc);

  forward[0] =  (pc * yc);
  forward[1] =  (pc * ys);
  forward[2] = -(ps);

  right[0] =  (ys * rc) - (ps * yc * rs);
  right[1] = -(yc * rc) - (ps * ys * rs);
  right[2] = -(pc * rs);

  up[0] = (ps * yc * rc) + (ys * rs);
  up[1] = (ps * ys * rc) - (yc * rs);
  up[2] = (pc * rc);
}
Esempio n. 14
0
void SinCos(float angle, float& sin, float& cos)
{
    float angleRadians = angle * M_DEGTORAD;
#if defined(HAVE_SINCOSF)
    sincosf(angleRadians, &sin, &cos);
#elif defined(HAVE_UNDERSCORE_SINCOSF)
    __sincosf(angleRadians, &sin, &cos);
#else
    sin = sinf(angleRadians);
    cos = cosf(angleRadians);
#endif
}
Esempio n. 15
0
void SharedBallModule::incorporateWorldModel(messages::WorldModel newModel) {
    if(newModel.ball_on()) {
        // heading + bearing
        float hb = TO_RAD*newModel.my_h() + TO_RAD*newModel.ball_bearing();
        float sinHB, cosHB;
        sincosf(hb, &sinHB, &cosHB);

        float globalX = newModel.my_x() + newModel.ball_dist()*cosHB;
        float globalY = newModel.my_y() + newModel.ball_dist()*sinHB;

        x = ALPHA*globalX + (1-ALPHA)*x;
        y = ALPHA*globalY + (1-ALPHA)*y;
        updatedThisFrame = true;
    }
}
Esempio n. 16
0
void matrixf_t::rotate(GLfloat a, GLfloat x, GLfloat y, GLfloat z)
{
    matrixf_t rotation;
    GLfloat* r = rotation.m;
    GLfloat c, s;
    r[3] = 0;   r[7] = 0;   r[11]= 0;
    r[12]= 0;   r[13]= 0;   r[14]= 0;   r[15]= 1;
    a *= GLfloat(M_PI / 180.0f);
    sincosf(a, &s, &c);
    if (isOnef(x) && isZerof(y) && isZerof(z)) {
        r[5] = c;   r[10]= c;
        r[6] = s;   r[9] = -s;
        r[1] = 0;   r[2] = 0;
        r[4] = 0;   r[8] = 0;
        r[0] = 1;
    } else if (isZerof(x) && isOnef(y) && isZerof(z)) {
        r[0] = c;   r[10]= c;
        r[8] = s;   r[2] = -s;
        r[1] = 0;   r[4] = 0;
        r[6] = 0;   r[9] = 0;
        r[5] = 1;
    } else if (isZerof(x) && isZerof(y) && isOnef(z)) {
        r[0] = c;   r[5] = c;
        r[1] = s;   r[4] = -s;
        r[2] = 0;   r[6] = 0;
        r[8] = 0;   r[9] = 0;
        r[10]= 1;
    } else {
        const GLfloat len = sqrtf(x*x + y*y + z*z);
        if (!isOnef(len)) {
            const GLfloat recipLen = reciprocalf(len);
            x *= recipLen;
            y *= recipLen;
            z *= recipLen;
        }
        const GLfloat nc = 1.0f - c;
        const GLfloat xy = x * y;
        const GLfloat yz = y * z;
        const GLfloat zx = z * x;
        const GLfloat xs = x * s;
        const GLfloat ys = y * s;
        const GLfloat zs = z * s;		
        r[ 0] = x*x*nc +  c;    r[ 4] =  xy*nc - zs;    r[ 8] =  zx*nc + ys;
        r[ 1] =  xy*nc + zs;    r[ 5] = y*y*nc +  c;    r[ 9] =  yz*nc - xs;
        r[ 2] =  zx*nc - ys;    r[ 6] =  yz*nc + xs;    r[10] = z*z*nc +  c;
    }
    multiply(rotation);
}
Esempio n. 17
0
int TestSinCosF(float a) {
    const float maxerr = 0.000000000001;
    float sincos_sin, sincos_cos, sin_sin, cos_cos;

    sincosf(a, &sincos_sin, &sincos_cos);
    sin_sin = sinf(a);
    cos_cos = cosf(a);
    if (fabsf(sincos_sin - sin_sin) > maxerr ||
            fabsf(sincos_cos - cos_cos) > maxerr) {
        printf("sincosf(%12.12f) outside tolerance: sin:%12.12f, cos:%12.12f\n",
               a, sincos_sin - sin_sin, sincos_cos - cos_cos);
        return 1;
    } else {
        return 0;
    }
}
Esempio n. 18
0
void textureAtlas::mathRotate(textureAtlasNode &p) noexcept
{
    vec2f offset[6] = {vec2f(0, 1), vec2f(1), vec2f(1, 0),
                       vec2f(1, 0), vec2f(0), vec2f(0, 1)};
    float msin, mcos;
    sincosf(p.angle, &msin, &mcos);

    for(int i = 0; i < 6; i++)
    {
        offset[i] -= p.center;
        offset[i] *= p.size;

        p.rotMatrix[i].x = offset[i].x*mcos-offset[i].y*msin;
        p.rotMatrix[i].y = offset[i].x*msin+offset[i].y*mcos;
    }
}
int
test_sincosf_abi (void)
{
  int i;
  for(i = 0; i < N; i++)
  {
    x[i] = i / 3;
    s_ptrs[i] = &s[i];
    c_ptrs[i] = &c[i];
  }

#pragma omp simd
  for(i = 0; i < N; i++)
    sincosf (x[i], s_ptrs[i], c_ptrs[i]);

  return 0;
}
Esempio n. 20
0
        __device__ __host__ __forceinline__ void AngleAxisf(float angle, const float3& r, float3& row1, float3& row2, float3& row3)
        {
            float cosA, sinA;
            sincosf(angle, &sinA, &cosA);

            row1.x = cosA;  row1.y =  0.f; row1.z =  0.f; 
            row2.x =  0.f;  row2.y = cosA; row2.z =  0.f; 
            row3.x =  0.f;  row3.y =  0.f; row3.z = cosA; 

            /*                 */  row1.y += -r.z * sinA; row1.z +=  r.y * sinA; 
            row2.x +=  r.z * sinA; /*                 */  row2.z += -r.x * sinA; 
            row3.x += -r.y * sinA; row3.y +=  r.x * sinA; /*                 */

            row1.x += r.x * r.x * (1 - cosA);  row1.y += r.x * r.y * (1 - cosA); row1.z += r.x * r.z * (1 - cosA); 
            row2.x += r.y * r.x * (1 - cosA);  row2.y += r.y * r.y * (1 - cosA); row2.z += r.y * r.z * (1 - cosA); 
            row3.x += r.z * r.x * (1 - cosA);  row3.y += r.z * r.y * (1 - cosA); row3.z += r.z * r.z * (1 - cosA);                             
        }
Esempio n. 21
0
/**
 * Updates the particle set according to the motion.
 *
 * @return the updated ParticleSet.
 */
void MotionSystem::update(ParticleSet& particles,
                          const messages::RobotLocation& odometry,
                          float error)
{
    // Store the last odometry and set the current one
    lastOdometry.set_x(curOdometry.x());
    lastOdometry.set_y(curOdometry.y());
    lastOdometry.set_h(curOdometry.h());
    curOdometry.set_x(odometry.x());
    curOdometry.set_y(odometry.y());
    curOdometry.set_h(odometry.h());

    // change in the robot frame
    float dX_R = curOdometry.x() - lastOdometry.x();
    float dY_R = curOdometry.y() - lastOdometry.y();
    float dH_R = curOdometry.h() - lastOdometry.h();

    if( (std::fabs(dX_R) > 3.f) || (std::fabs(dY_R) > 3.f) || (std::fabs(dH_R) > 0.35f) ) {
        // Probably reset odometry somewhere, so skip frame
        // std::cout << "std::fabs(dX_R) = " << std::fabs(dX_R) << " std::fabs(dY_R) = " << std::fabs(dY_R) << std::endl;
        // std::cout << "curOdometry.x() = " << curOdometry.x() << " curOdometry.y() = " << curOdometry.y() << std::endl;
        std::cout << "Odometry reset, motion frame skipped in loc" << std::endl;
        return;
    }

    float dX, dY, dH;
    ParticleIt iter;
    for(iter = particles.begin(); iter != particles.end(); iter++)
    {
        Particle* particle = &(*iter);

        // Rotate from the robot frame to the global to add the translation
        float sinh, cosh;
        sincosf(curOdometry.h() - particle->getLocation().h(),
                &sinh, &cosh);

        dX = (cosh*dX_R + sinh*dY_R) * FRICTION_FACTOR_X;
        dY = (cosh*dY_R - sinh*dX_R) * FRICTION_FACTOR_Y;
        dH = dH_R                    * FRICTION_FACTOR_H;

        particle->shift(dX, dY, dH);
        // noiseShiftWithOdo(particle, dX, dY, dH, error);
        randomlyShiftParticle(particle, false);
    }
}
Esempio n. 22
0
complex __powcc(float areal, float aimag, float breal, float bimag)
{
  float logr, logi, x, y;
  float sinx, cosx;
  complex r;

  logr = logf(hypotf(areal,aimag));
  logi = atan2f(aimag,areal);

  x = expf(logr*breal-logi*bimag);
  y = logr*bimag+logi*breal;

  sincosf(y, &sinx, &cosx);
  r.real = x*cosx;
  r.imag = x*sinx;

  return r;
}
Esempio n. 23
0
static TACommandVerdict sincosf_cmd(TAThread thread,TAInputStream stream)
{
    float x, s, c;

    x = readFloat(&stream);

    START_TARGET_OPERATION(thread);

    sincosf(x, &s, &c);

    END_TARGET_OPERATION(thread);

    writeFloat(thread, s);
    writeFloat(thread, c);
    sendResponse(thread);

    return taDefaultVerdict;
}
Esempio n. 24
0
bool enemy::update(hero *h)
{
    if(health > 0)
    {
        vec2f hpos = pos-h->getPos();
        angle = atan2(hpos.x, hpos.y);
        vec2f velocity;
        sincosf(angle, &velocity.x, &velocity.y);
        velocity *= window::deltaTime*speed;

        if(hpos < velocity)
            pos = h->getPos();
        else
            pos -= velocity;
        return 1;
    }
    time -= window::deltaTime;
    return 0;
}
Esempio n. 25
0
/**
 * Updates the particle set according to the motion.
 *
 * @return the updated ParticleSet.
 */
void MotionSystem::update(ParticleSet& particles,
                          const messages::RobotLocation& odometry,
                          bool nearMid)
{
    // Store the last odometry and set the current one
    lastOdometry.set_x(curOdometry.x());
    lastOdometry.set_y(curOdometry.y());
    lastOdometry.set_h(curOdometry.h());
    curOdometry.set_x(odometry.x());
    curOdometry.set_y(odometry.y());
    curOdometry.set_h(odometry.h());

    // change in the robot frame
    float dX_R = curOdometry.x() - lastOdometry.x();
    float dY_R = curOdometry.y() - lastOdometry.y();
    float dH_R = curOdometry.h() - lastOdometry.h();

    if( (std::fabs(dX_R) > 3.f) || (std::fabs(dY_R) > 3.f) ) {
        //Probably reset odometry somewhere so skip a frame
        return;
    }

    float dX, dY, dH;
    ParticleIt iter;
    for(iter = particles.begin(); iter != particles.end(); iter++)
    {
        Particle* particle = &(*iter);

        // Rotate from the robot frame to the global to add the translation
        float sinh, cosh;
        sincosf(curOdometry.h() - particle->getLocation().h(),
                &sinh, &cosh);

        dX = (cosh*dX_R + sinh*dY_R) * FRICTION_FACTOR_X;
        dY = (cosh*dY_R - sinh*dX_R) * FRICTION_FACTOR_Y;
        dH = dH_R                    * FRICTION_FACTOR_H; // just add the rotation

        particle->shift(dX, dY, dH);

        noiseShiftWithOdo(particle, dX, dY, dH);
        //randomlyShiftParticle(particle, nearMid);
    }
}
Esempio n. 26
0
void SharedBallModule::incorporateGoalieWorldModel(messages::WorldModel newModel) {
    if(newModel.ball_on()) {
        // heading + bearing

        // Assume goalie in position (FIELD_WHITE_LEFT_SIDELINE,
        //                            CENTER_FIELD_Y,
        //                            HEADING_RIGHT

        float hb = TO_RAD*HEADING_RIGHT + TO_RAD*newModel.ball_bearing();
        float sinHB, cosHB;
        sincosf(hb, &sinHB, &cosHB);

        float globalX = FIELD_WHITE_LEFT_SIDELINE_X + newModel.ball_dist()*cosHB;
        float globalY = CENTER_FIELD_Y + newModel.ball_dist()*sinHB;

        x = ALPHA*globalX + (1-ALPHA)*x;
        y = ALPHA*globalY + (1-ALPHA)*y;
        updatedThisFrame = true;
    }
}
Esempio n. 27
0
  //! Routine to set up z rotation
  __device__ void SetZRotation( const float theta )
  {
    /*!
      This routine sets the affine matrix locations
      corresponding to a rotation about the Z axis
      of the given angle.
      It leaves everything else untouched, so if you
      want a pure rotation matrix, call
      SetIdentity first.
    */
    if( threadIdx.x == 0 )
    {
      float s, c;
      sincosf( theta, &s, &c );

      (*this)(0,0) =  c;
      (*this)(0,1) =  s;
      (*this)(1,0) = -s;
      (*this)(1,1) =  c;
    }
  }
Esempio n. 28
0
int CHudAmmo::MsgFunc_Brass( const char *pszName, int iSize, void *pbuf )
{
	BEGIN_READ( pbuf, iSize );
	READ_BYTE();

	Vector start, velocity;
	start.x = READ_COORD();
	start.y = READ_COORD();
	start.z = READ_COORD();
	READ_COORD();
	READ_COORD(); // unused data
	READ_COORD();
	velocity.x = READ_COORD();
	velocity.y = READ_COORD();
	velocity.z = READ_COORD();

	float Rotation = M_PI * READ_ANGLE() / 180.0f;
	int ModelIndex = READ_SHORT();
	int BounceSoundType = READ_BYTE();
	int Life = READ_BYTE();
	READ_BYTE();

	float sin, cos, x, y;
	sincosf( Rotation, &sin, &cos );
	x = -9.0 * sin;
	y = 9.0 * cos;

	if( cl_righthand->value != 0.0f )
	{
		velocity.x += sin * -120.0;
		velocity.y += cos * 120.0;
		x = 9.0 * sin;
		y = -9.0 * cos;
	}

	start.x += x;
	start.y += y;
	EV_EjectBrass( start, velocity, Rotation, ModelIndex, BounceSoundType, Life );
	return 1;
}
Esempio n. 29
0
/**
 * @Brief - Main interface, takes in an update with a visionBall and a motion message
 *          Should be called whenever new information, and simply dont pass it the same
 *               message twice!
 * @params - visionball is a NEW vision message
           - motion is a NEW motion message
 */
void MMKalmanFilter::update(messages::VBall         visionBall,
                            messages::RobotLocation odometry)
{
    predictFilters(odometry);

    // Update with sensor if we have made an observation
    // sometimes get weird ball observatoins so check for valid dist (indicator)
    if(visionBall.on() && (visionBall.distance() > 5) && (visionBall.distance() < 800)) // We see the ball
    {
        //Before we mess with anything, decide if we saw the ball twice in a row
        consecutiveObservation = (framesWithoutBall == 0) ? true : false;

        // Update our visual observation history
        lastVisRelX = visRelX;
        lastVisRelY = visRelY;
        //Calc relx and rely from vision
        float sinB,cosB;
        sincosf(visionBall.bearing(),&sinB,&cosB);
        visRelX = visionBall.distance()*cosB;
        visRelY = visionBall.distance()*sinB;

        // Update our observation buffer for re-initializing the moving filter
        curEntry = (curEntry+1)%params.bufferSize;
        obsvBuffer[curEntry] = CartesianObservation(visRelX, visRelY);

        if (!fullBuffer && curEntry==0)
        { // If buffer wasnt full but is now
            fullBuffer = true;
        }

        // If we havent seen the ball for a long time, re-init our filters
        if (framesWithoutBall >= params.framesTillReset)
        {
            // Reset the filters
            initialize(visRelX, visRelY, params.initCovX, params.initCovY);

            // Reset relevant variables
            framesWithoutBall = 0;
        }

        // #HACK for competition - If we get into a bad observation cycle then change it
        else if (filters.at((unsigned)1)->getSpeed() > 700.f && consecutiveObservation){
            initialize(visRelX, visRelY, params.initCovX, params.initCovY);
        }

        // else if (fullBuffer) {
        //     // Calc velocity through these frames, if high reset moving filter
        //     CartesianObservation vel = calcVelocityOfBuffer();

        //     float speedThroughFrames = calcSpeed(vel.relX, vel.relY);

        //     // Calc diff between observation and estimata
        //     float estDiff = calcSpeed(visRelX - filters.at((unsigned)0)->getRelXPosEst(),
        //                               visRelY - filters.at((unsigned)0)->getRelYPosEst());

        //     // Much higher than our current estimate
        //     // if ((speedThroughFrames > filters.at((unsigned)1)->getSpeed() + 60.f)
        //     //     && (estDiff > params.badStationaryThresh))
        //     //if(estDiff > params.badStationaryThresh)

        //     // If moving velocity <10, give this a try
        //     if (std::abs(filters.at((unsigned)1)->getSpeed()) < 10.f
        //         && speedThroughFrames > 10.f)
        //     {
        //         //std::cout << "\nBall Kicked!" << std::endl;
        //         ufvector4 newMovingX = filters.at((unsigned)0)->getStateEst();
        //         newMovingX(2) = vel.relX;
        //         newMovingX(3) = vel.relY;
        //         ufmatrix4 newMovingCov = boost::numeric::ublas::identity_matrix <float>(4);
        //         newMovingCov(0,0) = 10.f;
        //         newMovingCov(1,1) = 10.f;
        //         newMovingCov(2,2) = 20.f;
        //         newMovingCov(3,3) = 20.f;

        //         filters.at((unsigned)1)->initialize(newMovingX, newMovingCov);
        //     }
        // }

        // Now correct our filters with the vision observation
        updateWithVision(visionBall);

        updatePredictions();
    }
    else {
        consecutiveObservation = false;

        // don't use/wipeout buffer
        fullBuffer = false;
        curEntry = 0;
    }

    // Determine filter
    if(TRACK_MOVEMENT) {
        float movingScore = filters.at((unsigned)1)->getScore();
        float stationaryScore = filters.at((unsigned)0)->getScore();
        if( (movingScore < stationaryScore))
        { // consider the ball to be moving
            bestFilter = 1;
        }
        else {
            bestFilter = 0;
        }
    }
    else {
        bestFilter = 0;
    }

    // Now update our estimates before housekeeping
    prevStateEst = stateEst;
    prevCovEst   = covEst;

    stateEst = filters.at((unsigned)bestFilter)->getStateEst();
    covEst   = filters.at((unsigned)bestFilter)->getCovEst();

    // Housekeep
    framesWithoutBall = (visionBall.on()) ? (0) : (framesWithoutBall+1);
    stationary = filters.at((unsigned)bestFilter)->isStationary();
}
Esempio n. 30
0
File: _cosf.c Progetto: scrpi/zasm
float cosf(float x) 
{
    if (x==0.0) return 1.0;
    return sincosf(x, 1);
}