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);
}
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();
}
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);
}
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;
}
}
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;
}
/* //////////////////////////////////////////////////////////////////////////////////////
* 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
}
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;
}
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);
}
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);
}
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;
}
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);
}
void P3DMath::SinCosf (float *sina,
float *cosa,
float a)
{
#ifdef HAVE_SINCOSF
sincosf(a,sina,cosa);
#else
*sina = sinf(a);
*cosa = cosf(a);
#endif
}
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);
}
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
}
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;
}
}
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);
}
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;
}
}
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;
}
__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);
}
/**
* 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);
}
}
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;
}
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;
}
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;
}
/**
* 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);
}
}
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;
}
}
//! 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;
}
}
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;
}
/**
* @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();
}
float cosf(float x)
{
if (x==0.0) return 1.0;
return sincosf(x, 1);
}
