void PlayerFirePowerArmor(Guy *me, byte mode)
{
int x, y, x2, y2;
byte f;
switch (mode) {
case 1:
MakeSound(SND_ARMORSHOOT, me->x, me->y, SND_CUTOFF, 1200);
f = me->facing * 32 - 64;
x = me->x + Cosine(me->facing * 32)*20;
y = me->y + Sine(me->facing * 32)*20;
x2 = x + Cosine(f)*32;
y2 = y + Sine(f)*32;
FireBullet(x2, y2, me->facing * 32, BLT_BIGSHELL, 1);
x2 = x - Cosine(f)*32;
y2 = y - Sine(f)*32;
FireBullet(x2, y2, me->facing * 32, BLT_BIGSHELL, 1);
if (player.ammo > 2)
player.ammo -= 2;
break;
case 2:
QuadMissile(me->x, me->y, me->facing, 1);
if (player.ammo > 25)
player.ammo -= 25;
else
player.ammo = 0;
break;
}
}
static void
inputs(superquadricsstruct * sp)
{
int iv;
double u, v, mode3, cn3, inverter2, flatu, flatv;
if (sp->Mode < 1.000001) {
mode3 = 1.0;
cn3 = 0.0;
inverter2 = 1.0;
} else if (sp->Mode < 2.000001) {
mode3 = 1.0;
cn3 = (sp->Mode - 1.0) * 1.5;
inverter2 = (sp->Mode - 1.0) * -2.0 + 1.0;
} else {
mode3 = (sp->Mode - 1.0);
cn3 = (sp->Mode - 2.0) / 2.0 + 1.5;
inverter2 = -1.0;
}
if (sp->flatshade) {
flatu = M_PI / (sp->resolution - 1);
flatv = mode3 * M_PI / ((sp->resolution - 1) * 2);
} else {
flatu = flatv = 0.0;
}
/* (void) printf("Calculating....\n"); */
for (iv = 1; iv <= sp->resolution; iv++) {
/* u ranges from PI down to -PI */
u = (1 - iv) * 2 * M_PI / (sp->resolution - 1) + M_PI;
/* v ranges from PI/2 down to -PI/2 */
v = (1 - iv) * mode3 * M_PI / (sp->resolution - 1) + M_PI * (mode3 / 2.0);
/* Use of xExponent */
sp->se[iv] = Sine(u, sp->xExponent);
sp->ce[iv] = Cosine(u, sp->xExponent);
sp->sn[iv] = Sine(v, sp->yExponent);
sp->cn[iv] = Cosine(v, sp->yExponent) * inverter2 + cn3;
/* Normal vector computations only */
sp->sw[iv] = Sine(u + flatu, 2 - sp->xExponent);
sp->cw[iv] = Cosine(u + flatu, 2 - sp->xExponent);
sp->ss[iv] = Sine(v + flatv, 2 - sp->yExponent) * inverter2;
sp->cs[iv] = Cosine(v + flatv, 2 - sp->yExponent);
} /* next */
/* Now fix up the endpoints */
sp->se[sp->resolution] = sp->se[1];
sp->ce[sp->resolution] = sp->ce[1];
if (sp->Mode > 2.999999) {
sp->sn[sp->resolution] = sp->sn[1];
sp->cn[sp->resolution] = sp->cn[1];
}
}
void UpdateRobotPos(void) {
int inc;
// If we are using the compass for robot direction, we simply assign the
// compass value to the robotPos.dir
// How to combine odometry and compass???????
// if (bUseCompassForDir)
// robotPos.dir = curCompassValue;
robotPosIncT += curSpeed[XSPEED]; // translational position
robotPosIncY += curSpeed[XSPEED]* Sine(robotPos.dir)/10000; // x position
robotPosIncX += curSpeed[XSPEED]* Cosine(robotPos.dir)/10000; // y position
robotPosIncW += curSpeed[WSPEED] + alignmentSpeed; // rotational position
inc = robotPosIncT/CD_1MM;
// uncertainty is a cheap way of telling us how accurate the robot's
// position is. The more the robot moves, the less accurate the position.
// Robot position is most accurate when it first started of and
// it has a chance to realign with external features, like detecting
// a known line.
if (inc) {
robotPosIncT -= CD_1MM*inc; // assume no negative XSPEED
uncertainty += inc;
}
inc = robotPosIncX/CD_1MM;
if (inc) {
robotPosIncX -= CD_1MM*inc;
robotPos.x += inc;
}
inc = robotPosIncY/CD_1MM;
if (inc) {
robotPosIncY -= CD_1MM*inc;
robotPos.y += inc;
}
// If compass is not used, the robot W-speed is used to estimate the change
// in robot direction.
// If the robot is well calibrated, the angle derived from odometry is more accurate
// in the short term. After a long series of moves (maybe more than a minute), it
// might be better to trust the compass reading more.
// It is important that there is no abrupt movement of robots in order for the
// robot position to be accurate. That is, there shouldn't be any fast acceleration
// or sudden top. If the robot is jerking, chances is that the robot position will
// not be accurate.
//if (!bUseCompassForDir)
//{
inc = robotPosIncW/CD_1DEG;
//DEBUGDATA0(robotPosIncW);
//DEBUGDATA1(inc);
//DEBUGDATA2(robotPos.dir);
if (inc) {
robotPosIncW -= CD_1DEG*inc;
robotPos.dir += inc*10;
}
// Limit the direction to +/- 180 degrees (i.e. -1800 to 1800)
// Unit is in 0.1 degree
Limit180Deg(robotPos.dir);
//}
}
Matrix4 rotate(Scalar angle, Vector4 axis)
{
axis.normalize();
Matrix4 tmp;
tmp.loadIdentity();
Scalar c = Cosine(angle);
Scalar s = Sine(angle);
tmp[0][0] = axis[0] * axis[0] * (1 - c) + c;
tmp[0][1] = axis[0] * axis[1] * (1 - c) - axis[2] * s;
tmp[0][2] = axis[0] * axis[2] * (1 - c) + axis[1] * s;
tmp[1][0] = axis[0] * axis[1] * (1 - c) + axis[2] * s;
tmp[1][1] = axis[1] * axis[2] * (1 - c) + c;
tmp[1][2] = axis[1] * axis[2] * (1 - c) - axis[0] * s;
tmp[2][0] = axis[0] * axis[2] * (1 - c) - axis[1] * s;
tmp[2][1] = axis[1] * axis[2] * (1 - c) + axis[0] * s;
tmp[2][2] = axis[2] * axis[2] * (1 - c) + c;
return tmp;
}
void Particle::Go(byte type,int x,int y,int z,byte angle,byte force)
{
byte fforce;
if(force==0)
return;
this->type=type;
size=2;
fforce=force/4;
if(fforce==0)
fforce=1;
this->x=x+MGL_randoml(32<<FIXSHIFT)-(16<<FIXSHIFT);
this->y=y+MGL_randoml(32<<FIXSHIFT)-(16<<FIXSHIFT);
this->z=z;
this->dx=Cosine(angle)*MGL_random(fforce);
this->dy=Sine(angle)*MGL_random(fforce);
this->dz=MGL_random(fforce*2)<<FIXSHIFT;
this->life=MGL_random(force)+10;
}
TermFactory::TermFactory() : ConstructionFactory<Term*>("Term") {
registerConstructor("", fl::null);
registerConstructor(Bell().className(), &(Bell::constructor));
registerConstructor(Binary().className(), &(Binary::constructor));
registerConstructor(Concave().className(), &(Concave::constructor));
registerConstructor(Constant().className(), &(Constant::constructor));
registerConstructor(Cosine().className(), &(Cosine::constructor));
registerConstructor(Discrete().className(), &(Discrete::constructor));
registerConstructor(Function().className(), &(Function::constructor));
registerConstructor(Gaussian().className(), &(Gaussian::constructor));
registerConstructor(GaussianProduct().className(), &(GaussianProduct::constructor));
registerConstructor(Linear().className(), &(Linear::constructor));
registerConstructor(PiShape().className(), &(PiShape::constructor));
registerConstructor(Ramp().className(), &(Ramp::constructor));
registerConstructor(Rectangle().className(), &(Rectangle::constructor));
registerConstructor(SShape().className(), &(SShape::constructor));
registerConstructor(Sigmoid().className(), &(Sigmoid::constructor));
registerConstructor(SigmoidDifference().className(), &(SigmoidDifference::constructor));
registerConstructor(SigmoidProduct().className(), &(SigmoidProduct::constructor));
registerConstructor(Spike().className(), &(Spike::constructor));
registerConstructor(Trapezoid().className(), &(Trapezoid::constructor));
registerConstructor(Triangle().className(), &(Triangle::constructor));
registerConstructor(ZShape().className(), &(ZShape::constructor));
}
/*!
\brief Returns alignment boolean.
Basically computes the cosine of the two vectors, and checks for unity.
*/
int Aligned(const Vector& u,const Vector& v)
{
double c=Cosine(u,v);
c*=c;
return (c>(1.0-0.0001));
}
void PlayerFireWeapon(Guy *me)
{
byte c;
if (player.life == 0)
return; // no shooting when you're dead
switch (player.weapon) {
case WPN_MISSILES:
if (player.ammo)
{
FireBullet(me->x, me->y, me->facing, BLT_MISSILE, 1);
player.ammo--;
}
player.wpnReload = 2;
break;
case WPN_BOMBS:
if (player.ammo)
{
FireBullet(me->x, me->y, me->facing, BLT_BOMB, 1);
player.ammo--;
}
player.wpnReload = 15;
break;
case WPN_AK8087:
if (player.ammo)
{
FireBullet(me->x, me->y, me->facing, BLT_LASER, 1);
player.ammo--;
}
me->z += FIXAMT * MGL_random(4);
me->dx += FIXAMT / 2 - MGL_random(65535);
me->dy += FIXAMT / 2 - MGL_random(65535);
c = GetControls();
if (c & CONTROL_B2) // fire is held
{
player.wpnReload = 1;
me->frmTimer = 0;
}
else
{
player.wpnReload = 5;
}
DoPlayerFacing(c, me);
break;
case WPN_FLAME:
if (player.ammo)
{
FireBullet(me->x, me->y, me->facing, BLT_FLAME, 1);
player.ammo--;
}
c = GetControls();
if (c & CONTROL_B2) // fire is held
{
player.wpnReload = 1;
me->frmTimer = 0;
}
else
player.wpnReload = 5;
DoPlayerFacing(c, me);
break;
case WPN_BIGAXE:
if (player.ammo)
{
FireBullet(me->x, me->y, me->facing, BLT_BIGAXE, 1);
MakeSound(SND_BOMBTHROW, me->x, me->y, SND_CUTOFF, 1200);
player.ammo--;
}
player.wpnReload = 10;
break;
case WPN_LIGHTNING:
if (player.ammo)
{
// fire lightning
FireBullet(me->x, me->y, me->facing, BLT_LIGHTNING, 1);
player.ammo--;
}
c = GetControls();
if (c & CONTROL_B2) // fire is held
{
player.wpnReload = 1;
me->frmTimer = 0;
}
else
{
player.wpnReload = 5;
}
DoPlayerFacing(c, me);
break;
case WPN_SPEAR:
if (player.ammo)
{
MakeSound(SND_BOMBTHROW, me->x, me->y, SND_CUTOFF, 1200);
FireBullet(me->x, me->y, me->facing, BLT_SPEAR, 1);
player.ammo--;
}
player.wpnReload = 5;
break;
case WPN_MACHETE:
if (player.ammo)
{
MakeSound(SND_SLASH, me->x, me->y, SND_CUTOFF, 1200);
FireBullet(me->x + Cosine(me->facing * 32)*32, me->y + Sine(me->facing * 32)*32,
me->facing, BLT_SLASH, 1);
player.ammo--;
}
player.wpnReload = 2;
break;
case WPN_MINES:
if (player.ammo)
{
MakeSound(SND_MINELAY, me->x, me->y, SND_CUTOFF, 1200);
FireBullet(me->x - Cosine(me->facing * 32)*32, me->y - Sine(me->facing * 32)*32,
me->facing, BLT_MINE, 1);
player.ammo--;
}
player.wpnReload = 15;
break;
case WPN_TURRET:
if (player.ammo)
{
Guy *g;
g = AddGuy(me->x + Cosine(me->facing * 32)*32, me->y + Sine(me->facing * 32)*32,
FIXAMT * 10, MONS_GOODTURRET);
if (g == NULL || !g->CanWalk(g->x, g->y, GameCurrentMap(), &curWorld))
{
MakeSound(SND_TURRETBZZT, me->x, me->y, SND_CUTOFF, 1200);
if (g)
g->type = MONS_NONE;
}
else
{
MakeSound(SND_MINELAY, me->x, me->y, SND_CUTOFF, 1200);
player.ammo--;
}
player.wpnReload = 15;
}
break;
case WPN_MINDCONTROL:
if (player.ammo)
{
MakeSound(SND_MINDWIPE, me->x, me->y, SND_CUTOFF, 1200);
FireBullet(me->x + Cosine(me->facing * 32)*32, me->y + Sine(me->facing * 32)*32,
me->facing, BLT_MINDWIPE, 1);
player.ammo--;
player.wpnReload = 15;
}
break;
case WPN_REFLECTOR:
if (player.ammo)
{
MakeSound(SND_LIGHTSON, me->x, me->y, SND_CUTOFF, 1200);
FireBullet(me->x, me->y, me->facing, BLT_REFLECT, 1);
player.ammo--;
c = GetControls();
if (c & CONTROL_B2) // fire is held
{
player.wpnReload = 0;
me->frmTimer = 0;
}
else
{
player.wpnReload = 10;
}
}
break;
case WPN_JETPACK:
if (player.ammo)
{
player.jetting = 5;
player.ammo--;
player.wpnReload = 3;
}
break;
case WPN_SWAPGUN:
if (player.ammo)
{
MakeSound(SND_LIGHTSON, me->x, me->y, SND_CUTOFF, 1200);
FireBullet(me->x, me->y, me->facing, BLT_SWAP, 1);
player.ammo--;
player.wpnReload = 10;
}
break;
}
if (!player.ammo)
player.weapon = 0;
}
void PlayerControlMe(Guy *me, mapTile_t *mapTile, world_t *world)
{
byte c;
int x, y, i;
player.life = me->hp;
if (player.rage)
{
if (player.rage > 5)
player.rage -= 6;
else
player.rage = 0;
}
if (player.rageClock)
DoRage(me);
if (player.invisibility)
player.invisibility--;
if (player.jetting && me->seq != ANIM_DIE && me->seq != ANIM_A3)
{
me->dx += Cosine(me->facing * 32)*6;
me->dy += Sine(me->facing * 32)*6;
Clamp(&me->dx, FIXAMT * 20);
Clamp(&me->dy, FIXAMT * 20);
if (me->z < FIXAMT * 20)
me->z += FIXAMT * 4;
me->dz = 0;
MakeSound(SND_FLAMEGO, me->x, me->y, SND_CUTOFF, 1200);
for (i = 0; i < 3; i++)
{
c = ((me->facing + 4)&7)*32;
x = me->x + Cosine(c)*10 - FIXAMT * 10 + MGL_randoml(FIXAMT * 20);
y = me->y + Sine(c)*10 - FIXAMT * 10 + MGL_randoml(FIXAMT * 20);
FireBullet(x, y, (me->facing + 4)&7, BLT_FLAME, 1);
}
player.jetting--;
}
if (player.weapon == WPN_PWRARMOR)
{
PlayerControlPowerArmor(me, mapTile, world);
return;
}
if (player.reload)
player.reload--;
if (player.wpnReload)
player.wpnReload--;
if (player.garlic)
{
player.garlic--;
StinkySteam(me->x - FIXAMT * 20 + MGL_randoml(FIXAMT * 40), me->y - FIXAMT * 20 + MGL_randoml(FIXAMT * 40),
me->z + FIXAMT * 40, FIXAMT * 2);
}
if (player.shield)
player.shield--;
if (player.pushPower)
player.pushPower--;
if (tportclock)
tportclock--;
// ice is slippery
if (!(world->terrain[mapTile->floor].flags & TF_ICE))
{
if (player.jetting && me->mind1)
{
if (me->mind1 & 1)
{
me->dx = -me->dx;
switch (me->facing) {
case 0:
me->facing = 4;
break;
case 1:
me->facing = 3;
break;
case 2:
case 6:
break;
case 3:
me->facing = 1;
break;
case 4:
me->facing = 0;
break;
case 5:
me->facing = 7;
break;
case 7:
me->facing = 5;
break;
}
}
if (me->mind1 & 2)
{
me->dy = -me->dy;
switch (me->facing) {
case 0:
case 4:
break;
case 1:
me->facing = 7;
break;
case 2:
me->facing = 6;
break;
case 3:
me->facing = 5;
break;
case 5:
me->facing = 3;
break;
case 6:
me->facing = 2;
break;
case 7:
me->facing = 1;
break;
}
}
}
Dampen(&me->dx, PLYR_DECEL);
Dampen(&me->dy, PLYR_DECEL);
}
else
{
if (me->mind1) // bumped a wall while on ice
{
if (me->mind1 & 1)
me->dx = -me->dx / 8;
if (me->mind1 & 2)
me->dy = -me->dy / 8;
}
}
me->mind1 = 0;
if (me->ouch == 4)
{
if (opt.playAs == PLAYAS_BOUAPHA)
{
if (me->hp > 0)
MakeSound(SND_BOUAPHAOUCH, me->x, me->y, SND_CUTOFF | SND_ONE, 2000);
else if (me->seq == ANIM_DIE) // so it doesn't do this if you're drowning
MakeSound(SND_BOUAPHADIE, me->x, me->y, SND_CUTOFF | SND_ONE, 2000);
}
else if (opt.playAs == PLAYAS_LUNATIC)
{
if (me->hp > 0)
MakeSound(SND_DRLOUCH, me->x, me->y, SND_CUTOFF | SND_ONE, 2000);
else if (me->seq == ANIM_DIE) // so it doesn't do this if you're drowning
MakeSound(SND_DRLDIE, me->x, me->y, SND_CUTOFF | SND_ONE, 2000);
}
else
{
if (me->hp > 0)
MakeSound(SND_HAPPYOUCH, me->x, me->y, SND_CUTOFF | SND_ONE, 2000);
else if (me->seq == ANIM_DIE) // so it doesn't do this if you're drowning
MakeSound(SND_HAPPYDIE, me->x, me->y, SND_CUTOFF | SND_ONE, 2000);
}
}
if (me->parent) // being grabbed by a Super Zombie or something
{
if (me->parent->type == MONS_SUPERZOMBIE)
{
me->dz = 0;
if (me->parent->frm < 4)
me->z += FIXAMT * 3;
else if (me->parent->frm > 18)
{
me->z -= FIXAMT * 4;
if (me->parent->frm == 21)
{
me->z = 0;
me->parent = NULL;
me->action = ACTION_IDLE;
if (me->hp == 0)
{
me->seq = ANIM_DIE;
me->frm = 0;
me->frmTimer = 0;
me->frmAdvance = 64;
me->action = ACTION_BUSY;
}
return;
}
}
if (me->seq != ANIM_MOVE)
{
me->seq = ANIM_MOVE;
me->frm = 0;
me->frmTimer = 0;
me->frmAdvance = 512;
}
return;
}
else if (me->parent->type == MONS_MINECART)
{
me->x = me->parent->x;
me->y = me->parent->y + 1;
me->z = FIXAMT * 8;
}
else
{
me->parent = NULL;
}
}
// triggering stuff
if (me->action == ACTION_BUSY)
{
// throw hammer if need be, use item if need be
if (me->seq == ANIM_A1 && me->frm == 2 && player.wpnReload == 0)
{
PlayerFireWeapon(me);
return;
}
if (me->seq == ANIM_A3)
{
if (me->frm < 11)
{
me->z = FIXAMT * 8; // hover while spinning feet in the air before plunging into water
me->dz = FIXAMT;
}
else
{
ExplodeParticles(PART_WATER, me->x, me->y, 0, 16);
}
return;
}
if (me->seq == ANIM_DIE)
{
me->facing = (me->facing + 1)&7;
return;
}
if (me->seq == ANIM_A1)
return;
}
// not busy, let's see if you want to do something
c = GetControls();
if (!player.jetting)
DoPlayerFacing(c, me);
if (me->action == ACTION_IDLE)
{
if ((c & (CONTROL_B1 | CONTROL_B2)) == (CONTROL_B1 | CONTROL_B2) && (player.rage / 256) >= player.life)
{
// RAGE!!!!!!!
player.rage = 0;
player.rageClock = 15;
if (player.shield == 0)
player.shield = 30;
EnterRage();
}
if ((c & CONTROL_B1) && player.reload == 0) // pushed hammer throw button
{
me->action = ACTION_IDLE;
if (!(c & (CONTROL_UP | CONTROL_DN | CONTROL_LF | CONTROL_RT)))
{
me->seq = ANIM_ATTACK; // even if unarmed
me->frm = 0;
me->frmTimer = 0;
me->frmAdvance = 255;
me->frm += 4 - (player.hamSpeed >> 2);
}
player.boredom = 0;
if (player.hammers > 0)
PlayerThrowHammer(me);
player.reload += (10 - (4 - (player.hamSpeed >> 2)));
}
int main(void)
{
uint8_t i, x, y;
MCU_initialize(); // initialize MCU and kit
Delay_ms(50); // wait for system stabilization
LCD_initialize(); // initialize text LCD module
Beep();
Clear_screen(); // initialize GLCD screen
LCD_string(0x80, "Graphic LCD Test"); // display title on text LCD
while (1) {
LCD_string(0xC0, " (1) Lines "); // graphic show 1
Rectangle(0, 0, 63, 127);
Delay_ms(300);
for (i = 0, x = 7, y = 15; i <= 7; i++, x += 8, y += 16) {
Line(x, 0, 0, y);
Line(0, 127 - y, x, 127);
Delay_ms(300);
}
for (i = 0, x = 7, y = 15; i <= 7; i++, x += 8, y += 16) {
Line(63, y, x, 127);
Line(x, 0, 63, 127 - y);
Delay_ms(300);
}
Delay_ms(3000);
Clear_screen();
LCD_string(0xC0, " (2) Rectangles "); // graphic show 2
for (i = 0, x = 0, y = 0; i <= 7; i++, x += 4, y += 8) {
Rectangle(x, y, 63 - x, 127 - y);
Delay_ms(300);
}
Delay_ms(1000);
for (i = 0; i <= 63; i++) { // (scroll up)
GLCD_command(0xC0, 0xC0 + i);
Delay_ms(100);
}
GLCD_command(0xC0, 0xC0);
Delay_ms(3000);
Clear_screen();
for (i = 0, x = 0, y = 0; i <= 7; i++, x += 4, y += 4) { // (two)
Rectangle(x, y, 63 - x, 63 - y);
Rectangle(x, y + 64, 63 - x, 127 - y);
Delay_ms(300);
}
Delay_ms(3000);
Clear_screen();
LCD_string(0xC0, " (3) Circles "); // graphic show 3
Rectangle(0, 0, 63, 127);
Delay_ms(300);
for (i = 7; i <= 63; i += 8) {
Circle(31, 63, i);
Delay_ms(300);
}
Delay_ms(1000);
for (i = 63; i != 0; i--) { // (scroll down)
GLCD_command(0xC0, 0xC0 + i);
Delay_ms(100);
}
GLCD_command(0xC0, 0xC0);
Delay_ms(3000);
Clear_screen();
for (i = 3; i <= 31; i += 4) { // (two)
Circle(31, 31, i);
Circle(31, 95, i);
Delay_ms(300);
}
Delay_ms(3000);
Clear_screen();
LCD_string(0xC0, " (4) Sine Curve "); // graphic show 4
Line(32, 0, 32, 127); // (X axis)
Line(29, 124, 32, 127);
Line(35, 124, 32, 127);
Line(31, 33, 33, 33);
Line(31, 63, 33, 63);
Line(31, 93, 33, 93);
Line(31, 123, 33, 123);
Line(0, 3, 63, 3); // (Y axis)
Line(3, 0, 0, 3);
Line(3, 6, 0, 3);
for (i = 5; i <= 30; i += 5) {
Sine(i);
Delay_ms(300);
}
Delay_ms(3000);
Clear_screen();
LCD_string(0xC0, "(5) Cosine Curve"); // graphic show 5
Line(32, 0, 32, 127); // (X axis)
Line(29, 124, 32, 127);
Line(35, 124, 32, 127);
Line(31, 33, 33, 33);
Line(31, 63, 33, 63);
Line(31, 93, 33, 93);
Line(31, 123, 33, 123);
Line(0, 3, 63, 3); // (Y axis)
Line(3, 0, 0, 3);
Line(3, 6, 0, 3);
for (i = 5; i <= 30; i += 5) {
Cosine(i);
Delay_ms(300);
}
Delay_ms(3000);
Clear_screen();
}
return 0;
}
void pWelch_rfvm(float newData, uint16_t* mean_freq1, uint16_t* mean_freq2, int start, int init)
{
static int EMGB_ptr1_1 = 0; // The two buffer used to make a delay
static int EMGB_ptr1_2 = 1;
static int welch_state; // Variable to hold the state
//static int mid_point; // After averaging over a certain amount of mean frequencies, that values is preserved as point for scaling
static int counter = 0; // This countetr keeps track of how many windows were processed from start to finish and used for averaging
//static float buff[30]; // Buffer to hold mean frequencies for averaging
//static float sum; // Holds the sum of mean frequencies
//static uint32_t averager = 0;
//static int pnt = 0;
static float coeffs_real_1[nfft]; // Buffer to hold real part of the coefficients for the first FFT window
static float coeffs_img_1[nfft]; // Buffer to hold imaginary part of the coefficients for the first FFT window
static float coeffs_real_2[nfft]; // Buffer to hold real part of the coefficients for the second FFT window
static float coeffs_img_2[nfft]; // Buffer to hold real part of the coefficients for the second FFT window
static float absolute_sum[nfft]; // Buffer to hold the sum of absolute value of the coefficients
float p = 2*PI/64;
static int c_1 = 0; // Counter the indicates how many samples inside a 64-sample window has been received and processed so far
static int c_2 = 0; // Counter is incremented with each new data, when it reaches 64, one window is complete, Counter is reset to zero and next window is started.
static int flag = 0;
float y1, y2;
int i, h;
float df = 7.8125; // 250 divided by 129 (256/2 + 1)
float f1, pow1, f2, pow2;
float temp1, temp2;
float *p1, *p2;
static int length = 0;
if (init == 0) // Not initializing
{
EMG_Buffer_1[EMGB_ptr1_1] = newData;
EMGB_ptr1_1++;
EMGB_ptr1_2++;
if (EMGB_ptr1_1 == FATIGUE_BUFFER-1)
EMGB_ptr1_1 = 0;
if (EMGB_ptr1_2 == FATIGUE_BUFFER-1)
EMGB_ptr1_2 = 0;
switch (welch_state)
{
case 0: // reset state
length = 0;
if (start == 1) // if start becomes 1, go to running state (start of a segment)
welch_state = 1;
break;
case 1:
length++;
temp1 = EMG_Buffer_1[EMGB_ptr1_2]*Hamming[c_1];
temp2 = p*c_1;
p1 = coeffs_real_1;
p2 = coeffs_img_1;
h = 9;
for (i = 0; i < nfft/4; i++) // with each incoming data, update all the coefficients, using loop unrolling
{
p1[0] += temp1*Cosine(temp2*h); // Real part of the coefficient
p2[0] -= -temp1*Sine(temp2*h); // Imaginary part of the coefficient
p1[1] += temp1*Cosine(temp2*(h+1));
p2[1] -= -temp1*Sine(temp2*(h+1));
p1[2] += temp1*Cosine(temp2*(h+2));
p2[2] -= -temp1*Sine(temp2*(h+2));
p1[3] += temp1*Cosine(temp2*(h+3));
p2[3] -= -temp1*Sine(temp2*(h+3));
p1 += 4;
p2 += 4;
h += 4;
}
c_1++; // Increment the counter
if (c_1 == overlap + 1)
flag = 1; // This means we have received 32 samples, it's time to start the concurrent calculation for overlaping windows
if (c_1 == SIZE) // we have reached the end of the window, coefficient calculation is complete, calculate absolute values and reset the counter
{
for (i = 0; i < nfft; i++)
{
absolute_sum[i] += coeffs_real_1[i]*coeffs_real_1[i] + coeffs_img_1[i]*coeffs_img_1[i]; // Finding the absoulte value for each coefficient
coeffs_real_1[i] = 0; // Setting to zero for the next window
coeffs_img_1[i] = 0; // Setting to zero for the next window
}
c_1 = 0;
counter++; // This countetr keeps track of how many windows were processed from start to finish and used for averaging
}
if (flag == 1) // Starting concurrent calculation for overlaping windows
{
temp1 = EMG_Buffer_1[EMGB_ptr1_2]*Hamming[c_2];
temp2 = p*c_2;
p1 = coeffs_real_2;
p2 = coeffs_img_2;
h = 9;
for (i = 0; i < nfft/4; i++) // with each incoming data, update all the coefficients
{
p1[0] += temp1*Cosine(temp2*h); // Real part of the coefficient
p2[0] -= -temp1*Sine(temp2*h); // Imaginary part of the coefficient
p1[1] += temp1*Cosine(temp2*(h+1));
p2[1] -= -temp1*Sine(temp2*(h+1));
p1[2] += temp1*Cosine(temp2*(h+2));
p2[2] -= -temp1*Sine(temp2*(h+2));
p1[3] += temp1*Cosine(temp2*(h+3));
p2[3] -= -temp1*Sine(temp2*(h+3));
p1 += 4;
p2 += 4;
h += 4;
}
c_2++; // Increment the counter
if (c_2 == SIZE)
{
for (i = 0; i < nfft; i++)
{
absolute_sum[i] += coeffs_real_2[i]*coeffs_real_2[i] + coeffs_img_2[i]*coeffs_img_2[i]; // Finding the absoulte value for each coefficient
coeffs_real_2[i] = 0; // Setting to zero for the next window
coeffs_img_2[i] = 0; // Setting to zero for the next window
}
c_2 = 0;
counter++;
}
}
if (start == 0) // End of segment, average the power spectrums, calculate mean frequency and reset the state machine parameters
{
y1 = 0;
f1 = 70.3125;
pow1 = 0;
y2 = 0;
f2 = 125;
pow2 = 0;
for (i = 0; i < nfft; i++) // for each coefficient, add the final two absolute values to the sum and find the average
{
c_1 = 0;
c_2 = 0;
flag = 0;
/*Don't add the last two*/
//absolute_sum[i] += coeffs_real_1[i]*coeffs_real_1[i] + coeffs_img_1[i]*coeffs_img_1[i];
//absolute_sum[i] += coeffs_real_2[i]*coeffs_real_2[i] + coeffs_img_2[i]*coeffs_img_2[i];
//absolute_sum[i] /= (counter);//(counter + 2);
if (i < 8)
{
pow1 += absolute_sum[i];
y1 += absolute_sum[i] * f1;
f1 += df;
}
if ( i > 7)
{
pow2 += absolute_sum[i];
y2 += absolute_sum[i] * f2;
f2 += df;
}
absolute_sum[i] = 0; // Reseting the buffers
coeffs_real_1[i] = 0;
coeffs_img_1[i] = 0;
coeffs_real_2[i] = 0;
coeffs_img_2[i] = 0;
}
if (length > 64)
{
MeanFreq_Q = (y1/ pow1)*256; // conveting to 2-byte fixed-point. normalize by 256 (max frequency) multiply by 2^16
*mean_freq1 = (y1/ pow1)*256;
*mean_freq2 = (y2/ pow2)*256;
}
welch_state = 0; // Go to reset state
length = 0;
}
break;
}
}
else // Initializing
{
welch_state = 0;
for (i = 0; i < nfft; i++) // Setting the buffers and pararmeters to zero
{
coeffs_real_1[i] = 0;
coeffs_img_1[i] = 0;
coeffs_real_2[i] = 0;
coeffs_img_2[i] = 0;
absolute_sum[i] = 0;
}
//sum = 0;
//averager = 0;
//pnt = 0;
//for (i = 0; i < 30; i++)
//{
// buff[i] = 0;
//}
}
}
int InRangeCosine(value *valueX, value *valueY, double R) {
return (Cosine(valueX, valueY) <= R);
}
int EqualCosine(value *valueX, value *valueY) {
return (Cosine(valueX, valueY) == 0);
}
int main(void)
{
float
x = 119,
y = 79;
unsigned char Input;
Model * m;
unsigned char Rocks = 0;
float scale;
ctorAllocator( ModelAllocator, ModelAllocator->Pool );
ctorAllocator( CommandAllocator, CommandAllocator->Pool );
//ctorAllocator( DLL_Allocator, DLL_Allocator->Pool );
//ctorDL_List( &ObjectList, DLL_Allocator, ObjectFree );
m = Copy( &mdlShip );
m->Position.x = 119;
m->Position.y = 79;
Bullet0.m = Copy( &mdlTriangle );
Bullet0.m->Position.x = 500;
Bullet0.m->Position.y = 500;
Rock0.m = Copy( &mdlRockA );
Rock1.m = Copy( &mdlRockB );
Rock2.m = Copy( &mdlRockC );
Rock0.InUse = 0;
Rock1.InUse = 0;
Rock2.InUse = 0;
smInit();
InitializeNESController();
//init_adc();
for(;;)
{
smFlip();
Input = ReadNESController();
m->Angle %= 360;
if( m->Angle < 0 )
m->Angle += 360;
if( Input & nesLeft )
--m->Angle;
if( Input & nesRight )
++m->Angle;
if( Input & nesStart )
{
m->Position.x = 119;
m->Position.y = 79;
m->Momentum = 0;
m->Angle = 0;
}
/*
if( Input & nesLeft && m->Angle != 270 )
if( m->Angle < 90 || m->Angle > 270 )
--m->Angle;
else
++m->Angle;
if( Input & nesRight && m->Angle != 90 )
if( m->Angle < 90 || m->Angle >= 270 )
++m->Angle;
else
--m->Angle;
if( Input & nesUp && m->Angle != 0 )
if( m->Angle < 180 )
--m->Angle;
else
++m->Angle;
if( Input & nesDown && m->Angle != 180 )
if( m->Angle < 180 )
++m->Angle;
else
--m->Angle;
*/
//if( !(rand() % 5) )
++Rock0.DrawAngle;
//if( !(rand() % 5) )
++Rock1.DrawAngle;
//if( !(rand() % 5) )
++Rock2.DrawAngle;
Bullet0.DrawAngle += 10;
if( rand() % 90 == 25 )
{
if( !Rock2.InUse )
{
Rock2.InUse = 1;
Rock2.m->Position.x = rand() % 240;
Rock2.m->Position.y = rand() % 160;
Rock2.m->Scale = 0.5F + 1.0F / (rand() % 100 + 1);
Rock2.m->Angle = rand() % 360;
Rock2.m->Momentum = rand() % 200;
}
else if( !Rock0.InUse )
{
Rock0.InUse = 1;
Rock0.m->Position.x = rand() % 240;
Rock0.m->Position.y = rand() % 160;
Rock0.m->Scale = 0.5F + 1.0F / (rand() % 100 + 1);
Rock0.m->Angle = rand() % 360;
Rock0.m->Momentum = rand() % 200;
}
else if( !Rock1.InUse )
{
Rock1.InUse = 1;
Rock1.m->Position.x = rand() % 240;
Rock1.m->Position.y = rand() % 160;
Rock1.m->Scale = 0.5F + 1.0F / (rand() % 100 + 1);
Rock1.m->Angle = rand() % 360;
Rock1.m->Momentum = rand() % 200;
}
}
if( Rock0.InUse )
DemiDraw( &Rock0 );
if( Rock1.InUse )
DemiDraw( &Rock1 );
if( Rock2.InUse )
DemiDraw( &Rock2 );
if( Input & nesA && (m->Momentum < 1000) )
m->Momentum += 10;
if( Input & nesDown && (m->Momentum > -1000) )
m->Momentum -= 10;
if( Input & nesB && !Bullet0.InUse )
{
Bullet0.m->Position.x = m->Position.x + (m->Radius - 27) * Sine(m->Angle);
Bullet0.m->Position.y = m->Position.y - (m->Radius - 27) * Cosine(m->Angle);
Bullet0.m->Momentum = 500 + m->Momentum;
Bullet0.m->Angle = m->Angle;
Bullet0.InUse = 1;
}
if( m->Momentum != 0 )
{
if( m->Momentum < 0 )
m->Momentum += 2;
else
m->Momentum -= 2;
}
m->Position.x += m->Momentum * .0025 * Sine(m->Angle);
m->Position.y += m->Momentum * .0025 * -Cosine(m->Angle);
/*
if( m->Position.x < 0 )
m->Position.x = 239;
if( m->Position.x > 239 )
m->Position.x = 0;
if( m->Position.y < 0 )
m->Position.y = 159;
if( m->Position.y > 159 )
m->Position.y = 0;
*/
Bullet0.m->Position.x += Bullet0.m->Momentum * .0025 * Sine(Bullet0.m->Angle);
Bullet0.m->Position.y += Bullet0.m->Momentum * .0025 * -Cosine(Bullet0.m->Angle);
if( Bullet0.m->Momentum != 0 )
{
if( Bullet0.m->Momentum < 0 )
Bullet0.m->Momentum += 2;
else
Bullet0.m->Momentum -= 2;
}
else
Bullet0.InUse = 0;
Rock0.m->Position.x += Rock0.m->Momentum * .0025 * Sine(Rock0.m->Angle);
Rock0.m->Position.y += Rock0.m->Momentum * .0025 * -Cosine(Rock0.m->Angle);
Rock1.m->Position.x += Rock1.m->Momentum * .0025 * Sine(Rock1.m->Angle);
Rock1.m->Position.y += Rock1.m->Momentum * .0025 * -Cosine(Rock1.m->Angle);
Rock2.m->Position.x += Rock2.m->Momentum * .0025 * Sine(Rock2.m->Angle);
Rock2.m->Position.y += Rock2.m->Momentum * .0025 * -Cosine(Rock2.m->Angle);
smColor( smBlue );
Draw( m );
if( Bullet0.InUse )
DemiDraw( &Bullet0 );
if( Rock0.InUse && Bullet0.InUse )
{
scale = Rock0.m->Radius * Rock0.m->Scale;
if( Bullet0.m->Position.x >= Rock0.m->Position.x - scale &&
Bullet0.m->Position.x <= Rock0.m->Position.x + scale &&
Bullet0.m->Position.y >= Rock0.m->Position.y - scale &&
Bullet0.m->Position.y <= Rock0.m->Position.y + scale )
{
Bullet0.InUse = 0;
Rock0.InUse = 0;
}
}
if( Rock1.InUse && Bullet0.InUse )
{
scale = Rock1.m->Radius * Rock1.m->Scale;
if( Bullet0.m->Position.x >= Rock1.m->Position.x - scale &&
Bullet0.m->Position.x <= Rock1.m->Position.x + scale &&
Bullet0.m->Position.y >= Rock1.m->Position.y - scale &&
Bullet0.m->Position.y <= Rock1.m->Position.y + scale )
{
Bullet0.InUse = 0;
Rock1.InUse = 0;
}
}
if( Rock2.InUse && Bullet0.InUse )
{
scale = Rock2.m->Radius * Rock2.m->Scale;
if( Bullet0.m->Position.x >= Rock2.m->Position.x - scale &&
Bullet0.m->Position.x <= Rock2.m->Position.x + scale &&
Bullet0.m->Position.y >= Rock2.m->Position.y - scale &&
Bullet0.m->Position.y <= Rock2.m->Position.y + scale )
{
Bullet0.InUse = 0;
Rock2.InUse = 0;
}
}
smWaitForFrame();
}
return 0;
}
void DoRage(Guy *me)
{
int cx, cy, i;
if (player.rageClock > 0)
player.rageClock--;
if (player.rageClock > 59)
switch (rageWpn) {
case WPN_NONE:
switch (opt.playAs) {
case PLAYAS_BOUAPHA:
if (player.rageClock == (player.rageClock / 4)*4)
HammerLaunch(me->x, me->y, me->facing, 5, HMR_REVERSE | HMR_REFLECT);
break;
case PLAYAS_LUNATIC:
if (player.rageClock == (player.rageClock / 4)*4)
{
for (i = 0; i < 10; i++)
FireBullet(me->x, me->y, (byte) MGL_random(8), BLT_BALLLIGHTNING, 1);
}
break;
case PLAYAS_HAPPY:
if (player.rageClock == (player.rageClock / 4)*4)
HappyLaunch(me->x, me->y, me->facing, 5, HMR_REVERSE | HMR_REFLECT);
break;
}
break;
case WPN_MISSILES:
FireBullet(me->x, me->y, (player.rageClock & 7), BLT_MISSILE, 1);
break;
case WPN_BOMBS:
GetCamera(&cx, &cy);
cx -= 320;
cy -= 240;
FireBullet((cx + MGL_random(640)) << FIXSHIFT, (cy + MGL_random(480)) << FIXSHIFT,
0, BLT_BOOM, 1);
ShakeScreen(10); // make the screen shake!
break;
case WPN_AK8087:
FireBullet(me->x, me->y, (byte) MGL_random(8), BLT_LASER, 1);
FireBullet(me->x, me->y, (byte) MGL_random(8), BLT_LASER, 1);
FireBullet(me->x, me->y, (byte) MGL_random(8), BLT_LASER, 1);
break;
case WPN_FLAME:
GetCamera(&cx, &cy);
cx -= 320;
cy -= 240;
for (i = 0; i < 3; i++)
FireBullet((cx + MGL_random(640)) << FIXSHIFT, (cy + MGL_random(480)) << FIXSHIFT,
(byte) MGL_random(8), BLT_FLAME, 1);
break;
case WPN_BIGAXE:
if (player.rageClock == (player.rageClock / 5)*5)
{
MakeSound(SND_BOMBTHROW, me->x, me->y, SND_CUTOFF, 1200);
FireBullet(me->x, me->y, me->facing, BLT_BIGAXE, 1);
}
break;
case WPN_LIGHTNING:
GetCamera(&cx, &cy);
cx -= 320;
cy -= 240;
FireBullet((cx + MGL_random(640)) << FIXSHIFT, (cy + MGL_random(480)) << FIXSHIFT,
(byte) MGL_random(8), BLT_LIGHTNING, 1);
break;
case WPN_SPEAR:
if (player.rageClock == (player.rageClock / 3)*3)
{
MakeSound(SND_BOMBTHROW, me->x, me->y, SND_CUTOFF, 1200);
FireBullet(me->x, me->y, (me->facing + 7)&7, BLT_SPEAR, 1);
FireBullet(me->x, me->y, me->facing, BLT_SPEAR, 1);
FireBullet(me->x, me->y, (me->facing + 1)&7, BLT_SPEAR, 1);
}
break;
case WPN_MACHETE:
GetCamera(&cx, &cy);
cx -= 320;
cy -= 240;
for (i = 0; i < 10; i++)
FireBullet((cx + MGL_random(640)) << FIXSHIFT, (cy + MGL_random(480)) << FIXSHIFT,
(byte) MGL_random(8), BLT_SLASH, 1);
break;
case WPN_MINES:
if (player.rageClock == (player.rageClock / 8)*8)
{
cx = 32 / 8 - ((player.rageClock - 60) / 8) + 1;
for (i = 0; i < 8; i++)
FireBullet(me->x + Cosine(i * 32)*(cx * 32), me->y + Sine(i * 32)*(cx * 32),
0, BLT_BOOM, 1);
}
break;
case WPN_MINDCONTROL:
if (player.rageClock & 1)
for (i = 0; i < 8; i++)
FireBullet(me->x, me->y, i, BLT_MINDWIPE, 1);
break;
case WPN_REFLECTOR:
FireBullet(me->x, me->y, 0, BLT_REFLECT, 1);
break;
case WPN_TURRET:
case WPN_SWAPGUN:
for (i = 0; i < 4; i++)
FireBullet(me->x, me->y, (i * 64 + player.rageClock)&255, BLT_GREEN, 1);
break;
case WPN_JETPACK:
for (i = 0; i < 8; i++)
FireBullet(me->x, me->y, i, BLT_FLAME, 1);
break;
}
}