void Astronomy::convertEquatorialToHorizontal (
double jday,
double longitude, double latitude,
double rasc, double decl,
double &azimuth, double &altitude)
{
double d = jday - 2451543.5;
double w = double (282.9404 + 4.70935E-5 * d);
double M = double (356.0470 + 0.9856002585 * d);
// Sun's mean longitude
double L = w + M;
// Universal time of day in degrees.
double UT = double(fmod(d, 1) * 360);
double hourAngle = longitude + L + double (180) + UT - rasc;
double x = cosDeg (hourAngle) * cosDeg (decl);
double y = sinDeg (hourAngle) * cosDeg (decl);
double z = sinDeg (decl);
double xhor = x * sinDeg (latitude) - z * cosDeg (latitude);
double yhor = y;
double zhor = x * cosDeg (latitude) + z * sinDeg (latitude);
azimuth = atan2Deg (yhor, xhor) + double (180);
altitude = atan2Deg (zhor, sqrt (xhor * xhor + yhor * yhor));
}
/*! This method returns the bisector (average) of two angles. It deals
with the boundary problem, thus when 'angMin' equals 170 and 'angMax'
equals -100, -145 is returned.
\param angMin minimum angle [-180,180]
\param angMax maximum angle [-180,180]
\return average of angMin and angMax. */
AngDeg getBisectorTwoAngles( AngDeg angMin, AngDeg angMax )
{
// separate sine and cosine part to circumvent boundary problem
return normalizeAngle(
atan2Deg( (sinDeg( angMin) + sinDeg( angMax ) )/2.0,
(cosDeg( angMin) + cosDeg( angMax ) )/2.0 ) );
}
AngDeg getBisectorTwoAngles( AngDeg angMin, AngDeg angMax ){
// separate sine and cosine part to circumvent boundary problem
//return Vector2D::normalizeAngle(
return normalizeTo180Deg( //wenns changed
atan2Deg( (sinDeg( angMin) + sinDeg( angMax ) )/2.0,
(cosDeg( angMin) + cosDeg( angMax ) )/2.0 ) );
}
void Astronomy::convertEquatorialToHorizontal (
LongReal jday,
LongReal longitude, LongReal latitude,
LongReal rasc, LongReal decl,
LongReal &azimuth, LongReal &altitude)
{
LongReal d = jday - 2451543.5;
LongReal w = LongReal (282.9404 + 4.70935E-5 * d);
LongReal M = LongReal (356.0470 + 0.9856002585 * d);
// Sun's mean longitude
LongReal L = w + M;
// Universal time of day in degrees.
LongReal UT = LongReal(fmod(d, 1) * 360);
LongReal hourAngle = longitude + L + LongReal (180) + UT - rasc;
LongReal x = cosDeg (hourAngle) * cosDeg (decl);
LongReal y = sinDeg (hourAngle) * cosDeg (decl);
LongReal z = sinDeg (decl);
LongReal xhor = x * sinDeg (latitude) - z * cosDeg (latitude);
LongReal yhor = y;
LongReal zhor = x * cosDeg (latitude) + z * sinDeg (latitude);
azimuth = atan2Deg (yhor, xhor) + LongReal (180);
altitude = atan2Deg (zhor, sqrt (xhor * xhor + yhor * yhor));
}
void Astronomy::convertSphericalToRectangular (
double rasc, double decl, double dist,
double &x, double &y, double &z)
{
x = dist * cosDeg (rasc) * cosDeg (decl);
y = dist * sinDeg (rasc) * cosDeg (decl);
z = dist * sinDeg (decl);
}
void Astronomy::convertSphericalToRectangular (
LongReal rasc, LongReal decl, LongReal dist,
LongReal &x, LongReal &y, LongReal &z)
{
x = dist * cosDeg (rasc) * cosDeg (decl);
y = dist * sinDeg (rasc) * cosDeg (decl);
z = dist * sinDeg (decl);
}
Vector3f getPosRelativeFromVision( VisionSense vision )
{
return Vector3f
(
vision.distance * cosDeg(vision.theta) * cosDeg(vision.phi),
vision.distance * sinDeg(vision.theta) * cosDeg(vision.phi),
vision.distance * sinDeg(vision.phi)
);
}
void Astronomy::getHorizontalSunPosition (
double jday,
double longitude, double latitude,
double &azimuth, double &altitude)
{
// 2451544.5 == Astronomy::getJulianDayFromGregorianDateTime(2000, 1, 1, 0, 0, 0));
// 2451543.5 == Astronomy::getJulianDayFromGregorianDateTime(1999, 12, 31, 0, 0, 0));
double d = jday - 2451543.5;
// Sun's Orbital elements:
// argument of perihelion
double w = double (282.9404 + 4.70935E-5 * d);
// eccentricity (0=circle, 0-1=ellipse, 1=parabola)
double e = 0.016709 - 1.151E-9 * d;
// mean anomaly (0 at perihelion; increases uniformly with time)
double M = double(356.0470 + 0.9856002585 * d);
// Obliquity of the ecliptic.
//double oblecl = double (23.4393 - 3.563E-7 * d);
// Eccentric anomaly
double E = M + radToDeg(e * sinDeg (M) * (1 + e * cosDeg (M)));
// Sun's Distance(R) and true longitude(L)
double xv = cosDeg (E) - e;
double yv = sinDeg (E) * sqrt (1 - e * e);
//double r = sqrt (xv * xv + yv * yv);
double lon = atan2Deg (yv, xv) + w;
double lat = 0;
double lambda = degToRad(lon);
double beta = degToRad(lat);
double rasc, decl;
convertEclipticToEquatorialRad (lambda, beta, rasc, decl);
rasc = radToDeg(rasc);
decl = radToDeg(decl);
// Horizontal spherical.
Astronomy::convertEquatorialToHorizontal (
jday, longitude, latitude, rasc, decl, azimuth, altitude);
}
Vector2D Vector2D::getVector2DFromPolar( float dMag, AngDeg ang ){
// cos(phi) = x/r <=> x = r*cos(phi); sin(phi) = y/r <=> y = r*sin(phi)
return ( Vector2D( dMag * cosDeg( ang ), dMag * sinDeg( ang ) ) );
}
void ai09::circle_ball ( int robot_num , float tagret_angle , int shoot_pow , int chip_pow , float precision , float near_dis_override )
{
//tagret_angle -= 5;
const float very_far_ball_dis = 600.0f;
const float far_ball_dis = 160.0f;
const int far_to_near_hys = 5;
float near_ball_dis = 140.0f;
if ( near_dis_override > 0 )
near_ball_dis = near_dis_override;
const float near_angle_tol = 4.0f;
const int near_to_kick_hys = 3;
const float shmit_coeff = 1.2f;
static ball_circling_state state = very_far;
static float last_change_t = 0.0f;
static int hys_bank[4]={0,0,0,0};
if (timer.time()<0.1) {
state = very_far;
last_change_t = timer.time();
hys_bank[0]=hys_bank[1]=hys_bank[2]=hys_bank[3]=0;
Halt(robot_num);
return;
}
if (state == very_far) {
OwnRobot[robot_num].face(ball.Position);
ERRTSetObstacles(robot_num, 0, 1, 1, 1, 0, 1);
ERRTNavigate2Point(robot_num, ball.Position, 1, 30, &VELOCITY_PROFILE_AROOM);
AddDebugCircle(ball.Position,very_far_ball_dis-90.0f,Red);
if (DIS(OwnRobot[robot_num].State.Position, ball.Position) < very_far_ball_dis) {
state = far;
last_change_t = timer.time();
}
}
else if (state == far) {
OwnRobot[robot_num].face(ball.Position);
ERRTSetObstacles(robot_num, 0, 1, 1, 1, 0, 1);
TVec2 target_point = CircleAroundPoint(ball.Position, AngleWith(ball.Position, OwnRobot[robot_num].State.Position), near_ball_dis);
ERRTNavigate2Point(robot_num, target_point, 1, 20, &VELOCITY_PROFILE_AROOM);
AddDebugCircle(ball.Position,far_ball_dis-90.0f,Pink);
if (DIS(OwnRobot[robot_num].State.Position, ball.Position) < far_ball_dis) {
hys_bank[0]++;
}
else {
hys_bank[0]=0;
}
if (hys_bank[0] > far_to_near_hys ) {
state = near;
last_change_t = timer.time();
}
else if (DIS(OwnRobot[robot_num].State.Position, ball.Position) > very_far_ball_dis * shmit_coeff) {
state = very_far;
last_change_t = timer.time();
}
}
else if (state == near) {
float toRobot = AngleWith(ball.Position, OwnRobot[robot_num].State.Position);
float newToRobot = NormalizeAngle(toRobot-tagret_angle);
float deltaAngle = min(fabs(newToRobot), 30.0f);
newToRobot = max(0.0f,fabs(newToRobot)-deltaAngle)*sgn(newToRobot);
newToRobot = NormalizeAngle(newToRobot+tagret_angle);
OwnRobot[robot_num].face(ball.Position);
OwnRobot[robot_num].target.Angle += NormalizeAngle(newToRobot+180.0f-OwnRobot[robot_num].target.Angle)/2.0f;
//OwnRobot[robot_num].target.Angle = NormalizeAngle(OwnRobot[robot_num].target.Angle);
ERRTSetObstacles(robot_num, 0, 1, 1, 1, 0, 1);
TVec2 target_point = CircleAroundPoint(ball.Position, newToRobot, near_ball_dis/cosDeg(deltaAngle));
if ( near_dis_override > 0 )
ERRTNavigate2Point(robot_num, target_point, 1, 20, &VELOCITY_PROFILE_AROOM);
else
ERRTNavigate2Point(robot_num, target_point, 1, 20, &VELOCITY_PROFILE_MAMOOLI);
if (DIS(OwnRobot[robot_num].State.Position, ball.Position) > far_ball_dis*shmit_coeff) {
state = far;
last_change_t = timer.time();
}
if (fabs(deltaAngle) < near_angle_tol) {
hys_bank[0]++;
}
else {
hys_bank[0]=0;
}
if ((hys_bank[0]>near_to_kick_hys)&&((shoot_pow>0)||(chip_pow>0))) {
state = kick;
last_change_t = timer.time();
}
}
else if (state == kick) {
if (chip_pow>0) {
chip_head = OwnRobot[robot_num].State.Angle;
}
//OwnRobot[robot_num].face(ball.Position);
OwnRobot[robot_num].target.Angle=NormalizeAngle(tagret_angle+180.0f);
ERRTSetObstacles(robot_num, 0, 1, 1, 1, 0, 1);
ERRTNavigate2Point(robot_num, ball.Position, 1, 100, &VELOCITY_PROFILE_AROOM);
if ( shoot_pow > 0 )
OwnRobot[robot_num].Shoot(shoot_pow);
if ( chip_pow > 0 )
OwnRobot[robot_num].Chip(chip_pow);
AddDebugCircle(ball.Position,very_far_ball_dis-90.0f,Red);
//tech_circle(robot_num, tagret_angle, shoot_pow, chip_pow, 1, 1, 0, 0);
if (DIS(OwnRobot[robot_num].State.Position, ball.Position) > near_ball_dis*shmit_coeff) {
state = far;
last_change_t = timer.time();
}
}
AddDebugLine(OwnRobot[robot_num].State.Position,OwnRobot[robot_num].target.Position,Black);
}
void gameUpdate(void) {
point vertex, port, starboard, exhaust, playerPos;
point playerShip[5];
int i, j;
//Level defaults to finished, changed when rocks or enemies are alive.
HWREGBITW(&gFlags, LEVEL_COMPLETE) = True;
//Update player
gPlayer.x += gPlayer.dx;
gPlayer.y += gPlayer.dy;
gPlayer.x = floatMod(gPlayer.x, 128);
gPlayer.y = floatMod(gPlayer.y, 96);
gPlayer.dx = (gPlayer.dx*SPEED_DECAY);
gPlayer.dy = (gPlayer.dy*SPEED_DECAY);
gPlayer.exhaustOn = False;
playerPos = makePoint((int)gPlayer.x, (int)gPlayer.y);
vertex = rotPoint(playerPos, gPlayer.angle,
makePoint(playerPos.x+6, playerPos.y));
port = rotPoint(playerPos, gPlayer.angle,
makePoint(playerPos.x-5, playerPos.y-5));
starboard = rotPoint(playerPos, gPlayer.angle,
makePoint(playerPos.x-5, playerPos.y+5));
exhaust = rotPoint(playerPos, gPlayer.angle,
makePoint(playerPos.x-3, playerPos.y));
playerShip[0] = vertex;
playerShip[1] = port;
playerShip[2] = exhaust;
playerShip[3] = starboard;
playerShip[4] = makePoint((int)gPlayer.x, (int)gPlayer.y);
switch(gPlayer.status) {
case ALIVE:
////Button movement input
//Forward (up)
if ((GPIO_PORTG_DATA_R&0x08) == 0 || HWREGBITW(&gFlags, ANALOG_UP)) {
if((gPlayer.dx*gPlayer.dx + gPlayer.dy*gPlayer.dy) <
MAX_PLAYER_SPEED*MAX_PLAYER_SPEED) {
gPlayer.dx += cosDeg(gPlayer.angle)*PLAYER_ACCEL;
gPlayer.dy -= sinDeg(gPlayer.angle)*PLAYER_ACCEL;
}
gPlayer.exhaustOn = True;
}
//Left
if((GPIO_PORTG_DATA_R&0x20) == 0) {//|| HWREGBITW(&gFlags, ANALOG_LEFT)) {
gPlayer.angle += PLAYER_TURN_RATE;
}
//Right
if((GPIO_PORTG_DATA_R&0x40) == 0) {// || HWREGBITW(&gFlags, ANALOG_RIGHT)) {
gPlayer.angle -= PLAYER_TURN_RATE;
}
//Select (positive edge)
if((GPIO_PORTG_DATA_R & 0x80) != 0) {
selectNotPressed = True;
}
if(HWREGBITW(&gFlags, SELECT_DOWN) == 1 ||
(selectNotPressed == True && (GPIO_PORTG_DATA_R & 0x80) == 0)) {
selectNotPressed = False;
if(HWREGBITW(&gFlags, TITLE_SCREEN) == True) {
HWREGBITW(&gFlags, TITLE_SCREEN) = False;
setXYAvg();
}
HWREGBITW(&gFlags, SELECT_DOWN) = 0; //reset flag
addBullet(makePoint((int)gPlayer.x, (int)gPlayer.y),
(cosDeg(gPlayer.angle)*MAX_BULLET_SPEED),
-1*(sinDeg(gPlayer.angle)*MAX_BULLET_SPEED),
True);
}
break;
case HIT:
gPlayer.status = DEAD;
addExplosion(makePoint(gPlayer.x, gPlayer.y), 12);
gPlayer.dx = 0;
gPlayer.dy = 0;
killBullets();
killRocks();
break;
case DEAD:
//Wait for explosions to stop, then stop updating the game.
for(i = 0; i < MAX_EXPLOSIONS; i++) {
if(gExplosions[i].status == ALIVE) { break; }
}
HWREGBITW(&gFlags, LEVEL_COMPLETE) = False;
HWREGBITW(&gFlags, GAME_OVER) = True;
killRocks();
killBullets();
killEnemies();
return;
}
if(HWREGBITW(&gFlags, TITLE_SCREEN) == True) {
return;
}
//Update bullets
for(i = 0; i < MAX_PLAYER_BULLETS; i++) {
switch(gPlayerBullets[i].status) { //Only update visible bullets.
case ALIVE:
if(gPlayerBullets[i].life++ > BULLET_LIFETICKS) {
gPlayerBullets[i].status = DEAD;
break;
}
gPlayerBullets[i].x = gPlayerBullets[i].x+gPlayerBullets[i].dx;
gPlayerBullets[i].y = gPlayerBullets[i].y+gPlayerBullets[i].dy;
break;
case HIT:
break;
case DEAD:
break;
}
}
for(i = 0; i < MAX_ENEMY_BULLETS; i++) {
switch(gEnemyBullets[i].status) { //Only update visible bullets.
case ALIVE:
if(gEnemyBullets[i].life++ > BULLET_LIFETICKS) {
gEnemyBullets[i].status = DEAD;
break;
}
HWREGBITW(&gFlags, LEVEL_COMPLETE) = False;
gEnemyBullets[i].x = gEnemyBullets[i].x+gEnemyBullets[i].dx;
gEnemyBullets[i].y = gEnemyBullets[i].y+gEnemyBullets[i].dy;
break;
case HIT:
break;
case DEAD:
break;
}
}
//Update rocks
for(i = 0; i < MAX_ROCKS; i++) {
switch(gRocks[i].status) {
case ALIVE: //Only update visible rocks.
HWREGBITW(&gFlags, LEVEL_COMPLETE) = False;
//Update rock position
if(gRocks[i].dx < 0.1 && gRocks[i].dy < 0.1) {
gRocks[i].dx = (randRange(32,64)*-1+randRange(32,64)*1)/64.;
gRocks[i].dy = randRange(0,256)/256;
}
gRocks[i].x = floatMod(gRocks[i].x+gRocks[i].dx, 128);
gRocks[i].y = floatMod(gRocks[i].y+gRocks[i].dy, 96);
//Check collisions with enemies, players and bullets.
//Player collision
for(j = 0; j < 5; j++) {
if(pointInRock(makePoint(((int)gRocks[i].x),
((int)gRocks[i].y)),
gRocks[i].rockType,
gRocks[i].rockSize,
playerShip[j])) {
gPlayer.status = HIT;
return;
}
}
//Bullet collision
for(j = 0; j < MAX_PLAYER_BULLETS; j++) {
if(gPlayerBullets[j].status == ALIVE) {
if(pointInRock(makePoint(((int)gRocks[i].x),
((int)gRocks[i].y)),
gRocks[i].rockType,
gRocks[i].rockSize,
makePoint(((int)gPlayerBullets[j].x),
((int)gPlayerBullets[j].y)))) {
score += 1;
addExplosion(makePoint(gPlayerBullets[j].x,
gPlayerBullets[j].y), 2);
gRocks[i].status = HIT;
gPlayerBullets[j].status = DEAD;
}
}
}/*
for(j = 0; j < MAX_ENEMY_BULLETS; j++) {
if(gEnemyBullets[i].status == ALIVE) {
if(pointInRock(makePoint(((int)gRocks[i].x),
((int)gRocks[i].y)),
gRocks[i].rockType,
gRocks[i].rockSize,
makePoint(((int)gEnemyBullets[j].x)%128,
((int)gEnemyBullets[j].y)%96))) {
gRocks[i].status = HIT;
addExplosion(makePoint(gEnemyBullets[j].x,
gEnemyBullets[j].y), 2);
gEnemyBullets[j].status = DEAD;
}
}
}*/
if(gRocks[i].status == ALIVE) { break; }
case HIT:
if(gRocks[i].rockSize > 1) {
addRock(makePoint((int)gRocks[i].x+1, (int)gRocks[i].y+1),
randRange(24, 64)/-64.,
randRange(24, 64)/-64.,
gRocks[i].rockSize-1);
addRock(makePoint((int)gRocks[i].x-1, (int)gRocks[i].y-1),
randRange(64, 24)/64.,
randRange(64, 24)/64.,
gRocks[i].rockSize-1);
}
gRocks[i].status = DEAD;
break;
case DEAD:
break;
}
}
//Update UFOs
for(i = 0; i < MAX_UFOS; i++) {
switch(gUFOs[i].status) {
case ALIVE: //Only update visible rocks.
HWREGBITW(&gFlags, LEVEL_COMPLETE) = False;
//Update rock position
if(gUFOs[i].dx < 0.1 && gUFOs[i].dy < 0.1) {
gUFOs[i].dx = (randRange(32,64)*-1+randRange(32,64)*1)/64.;
gUFOs[i].dy = randRange(0,256)/256;
}
gUFOs[i].pos.x = floatMod(gUFOs[i].pos.x+gUFOs[i].dx, 128);
gUFOs[i].pos.y = floatMod(gUFOs[i].pos.y+gUFOs[i].dy, 96);
if(gUFOs[i].status == ALIVE) { break; }
case HIT:
gUFOs[i].status = DEAD;
break;
case DEAD:
break;
}
}
//Update explosions
for(i = 0; i < MAX_EXPLOSIONS; i++) {
if(gExplosions[i].status == ALIVE) {
HWREGBITW(&gFlags, LEVEL_COMPLETE) = False;
if(gExplosions[i].current++ > gExplosions[i].lifetime) {
gExplosions[i].status = DEAD;
continue;
}
}
}
}
