trigreal X(tan2pi)(int m, int n)
{
#if 0 /* unimplemented, unused */
trigreal dm = m, dn = n;
return TAN(by2pi(dm, dn));
#endif
UNUSED(m); UNUSED(n);
return 0.0;
}
static float ray_caster_v(t_data *d, float ray_a)
{
float int_vy;
float int_vx;
float xa;
float ya;
if (ray_a <= NORTH || ray_a > SOUTH)
int_vx = (d->p.x / CUBE) * (CUBE) + CUBE;
else
int_vx = (d->p.x / CUBE) * (CUBE) - 1;
int_vy = d->p.y + (d->p.x - int_vx) * TAN(ray_a);
xa = (ray_a <= NORTH || ray_a > SOUTH) ? CUBE : -CUBE;
ya = -xa * TAN(ray_a);
while ((int_vx >= 0 && int_vx < CUBE * MAP_H)
&& (int_vy >= 0 && int_vy < CUBE * MAP_H))
{
if (d->map[(int)(int_vy / CUBE)][(int)(int_vx / CUBE)] != 0)
return (sqrt(pow(d->p.x - int_vx, 2) + pow(d->p.y - int_vy, 2)));
int_vx += xa;
int_vy += ya;
}
return (MAX_DS);
}
static float ray_caster_h(t_data *d, float ray_a)
{
float int_hy;
float int_hx;
float xa;
float ya;
if (ray_a <= WEST)
int_hy = (d->p.y / CUBE) * (CUBE) - 1;
else
int_hy = (d->p.y / CUBE) * (CUBE) + CUBE;
int_hx = d->p.x + (d->p.y - int_hy) / TAN(ray_a);
ya = (ray_a <= WEST) ? -CUBE : CUBE;
xa = -ya / TAN(ray_a);
while ((int_hx >= 0 && int_hx < CUBE * MAP_H)
&& (int_hy >= 0 && int_hy < CUBE * MAP_H))
{
if (d->map[(int)(int_hy / CUBE)][(int)(int_hx / CUBE)] != 0)
return (sqrt(pow(d->p.x - int_hx, 2) + pow(d->p.y - int_hy, 2)));
int_hx += xa;
int_hy += ya;
}
return (MAX_DS);
}
Matrix3& Matrix3::PerspectiveRH( const Real fov, const Real aspect, const Real zn, const Real zf )
{
#ifdef __USE_D3DX__
D3DXMatrixPerspectiveFovRH( (D3DXMATRIX*)this, fov, aspect, zn, zf );
#else
Real h = 1.0f / TAN( fov * 0.5f );
Real w = h / aspect;
Real Q = zf / ( zn - zf );
Zero();
e[0] = w;
e[5] = h;
e[10] = Q;
e[14] = Q * zn;
e[11] = -1.0;
#endif
return *this;
}
static void
sunpos(int inYY, int inMM, int inDD, double UTCOFFSET, int inHOUR, int inMIN,
int inSEC, double eastlongitude, double latitude, double *L, double *DEC)
{
int Y;
double ZJ, D, T, M, epsilon, lambda, alpha, HA, UTHM;
ZJ = ZJtable[inMM];
if (inMM <= 2 && isleap(inYY))
ZJ -= 1.0;
UTHM = inHOUR + inMIN / FMINSPERHOUR + inSEC / FSECSPERHOUR - UTCOFFSET;
Y = inYY - 1900; /* 1 */
D = floor(365.25 * Y) + ZJ + inDD + UTHM / FHOURSPERDAY; /* 3 */
T = D / 36525.0; /* 4 */
*L = 279.697 + 36000.769 * T; /* 5 */
fixup(L);
M = 358.476 + 35999.050 * T; /* 6 */
fixup(&M);
epsilon = 23.452 - 0.013 * T; /* 7 */
fixup(&epsilon);
lambda = *L + (1.919 - 0.005 * T) * SIN(M) + 0.020 * SIN(2 * M);/* 8 */
fixup(&lambda);
alpha = ATAN(TAN(lambda) * COS(epsilon)); /* 9 */
/* Alpha should be in the same quadrant as lamba */
{
int lssign = sin(D2R(lambda)) < 0 ? -1 : 1;
int lcsign = cos(D2R(lambda)) < 0 ? -1 : 1;
while (((sin(D2R(alpha)) < 0) ? -1 : 1) != lssign
|| ((cos(D2R(alpha)) < 0) ? -1 : 1) != lcsign)
alpha += 90.0;
}
fixup(&alpha);
*DEC = ASIN(SIN(lambda) * SIN(epsilon)); /* 10 */
fixup(DEC);
fixup(&eastlongitude);
HA = *L - alpha + 180 + 15 * UTHM + eastlongitude; /* 12 */
fixup(&HA);
fixup(&latitude);
#ifdef NOTDEF
printf("%02d/%02d %02d:%02d:%02d l:%g d:%g h:%g\n",
inMM, inDD, inHOUR, inMIN, inSEC, latitude, *DEC, HA);
#endif
return;
/*
* The following calculations are not used, so to save time
* they are not calculated.
*/
#ifdef NOTDEF
*ALT = ASIN(SIN(latitude) * SIN(*DEC) +
COS(latitude) * COS(*DEC) * COS(HA)); /* 13 */
fixup(ALT);
*AZ = ATAN(SIN(HA) /
(COS(HA) * SIN(latitude) - TAN(*DEC) * COS(latitude))); /* 14 */
if (*ALT > 180)
*ALT -= 360;
if (*ALT < -180)
*ALT += 360;
printf("a:%g a:%g\n", *ALT, *AZ);
#endif
#ifdef NOTDEF
printf("Y:\t\t\t %d\t\t %d\t\t %d\n", Y, expY, Y - expY);
comp("ZJ", ZJ, expZJ);
comp("UTHM", UTHM, expUTHM);
comp("D", D, expD);
comp("T", T, expT);
comp("L", L, fixup(&expL));
comp("M", M, fixup(&expM));
comp("epsilon", epsilon, fixup(&expepsilon));
comp("lambda", lambda, fixup(&explambda));
comp("alpha", alpha, fixup(&expalpha));
comp("DEC", DEC, fixup(&expDEC));
comp("eastlongitude", eastlongitude, fixup(&expeastlongitude));
comp("latitude", latitude, fixup(&explatitude));
comp("HA", HA, fixup(&expHA));
comp("ALT", ALT, fixup(&expALT));
comp("AZ", AZ, fixup(&expAZ));
#endif
}
/* TODO: a way to control the speed */
void
WaterScreen::preparePaint (int msSinceLastPaint)
{
if (count)
{
count -= 10;
if (count < 0)
count = 0;
if (wiperTimer.active ())
{
float step, angle0, angle1;
bool wipe = false;
XPoint p[3];
p[1].x = screen->width () / 2;
p[1].y = screen->height ();
step = wiperSpeed * msSinceLastPaint / 20.0f;
if (wiperSpeed > 0.0f)
{
if (wiperAngle < 180.0f)
{
angle0 = wiperAngle;
wiperAngle += step;
wiperAngle = MIN (wiperAngle, 180.0f);
angle1 = wiperAngle;
wipe = true;
}
}
else
{
if (wiperAngle > 0.0f)
{
angle1 = wiperAngle;
wiperAngle += step;
wiperAngle = MAX (wiperAngle, 0.0f);
angle0 = wiperAngle;
wipe = true;
}
}
#define TAN(a) (tanf ((a) * (M_PI / 180.0f)))
if (wipe)
{
if (angle0 > 0.0f)
{
p[2].x = screen->width () / 2 -
screen->height () / TAN (angle0);
p[2].y = 0;
}
else
{
p[2].x = 0;
p[2].y = screen->height ();
}
if (angle1 < 180.0f)
{
p[0].x = screen->width () / 2 -
screen->height () / TAN (angle1);
p[0].y = 0;
}
else
{
p[0].x = screen->width ();
p[0].y = screen->height ();
}
waterVertices (GL_TRIANGLES, p, 3, 0.0f);
}
#undef TAN
}
waterUpdate (0.8f);
}
cScreen->preparePaint (msSinceLastPaint);
}
/* TODO: a way to control the speed */
static void waterPreparePaintScreen(CompScreen * s, int msSinceLastPaint)
{
WATER_SCREEN(s);
if (ws->count) {
ws->count -= 10;
if (ws->count < 0)
ws->count = 0;
if (ws->wiperHandle) {
float step, angle0, angle1;
Bool wipe = FALSE;
XPoint p[3];
p[1].x = s->width / 2;
p[1].y = s->height;
step = ws->wiperSpeed * msSinceLastPaint / 20.0f;
if (ws->wiperSpeed > 0.0f) {
if (ws->wiperAngle < 180.0f) {
angle0 = ws->wiperAngle;
ws->wiperAngle += step;
ws->wiperAngle =
MIN(ws->wiperAngle, 180.0f);
angle1 = ws->wiperAngle;
wipe = TRUE;
}
} else {
if (ws->wiperAngle > 0.0f) {
angle1 = ws->wiperAngle;
ws->wiperAngle += step;
ws->wiperAngle =
MAX(ws->wiperAngle, 0.0f);
angle0 = ws->wiperAngle;
wipe = TRUE;
}
}
#define TAN(a) (tanf ((a) * (M_PI / 180.0f)))
if (wipe) {
if (angle0 > 0.0f) {
p[2].x =
s->width / 2 -
s->height / TAN(angle0);
p[2].y = 0;
} else {
p[2].x = 0;
p[2].y = s->height;
}
if (angle1 < 180.0f) {
p[0].x =
s->width / 2 -
s->height / TAN(angle1);
p[0].y = 0;
} else {
p[0].x = s->width;
p[0].y = s->height;
}
/* software rasterizer doesn't support triangles yet so wiper
effect will only work with FBOs right now */
waterVertices(s, GL_TRIANGLES, p, 3, 0.0f);
}
#undef TAN
}
waterUpdate(s, 0.8f);
}
UNWRAP(ws, s, preparePaintScreen);
(*s->preparePaintScreen) (s, msSinceLastPaint);
WRAP(ws, s, preparePaintScreen, waterPreparePaintScreen);
}
int main ( int argc, char * argv[] )
/* main: handle options, open files */
{
double tg, pr, l, crr, fa, tl, hl, t;
char * scale;
char * desc;
double a10, a11, a20, a21;
coOrd q0, q1, q2, q3, q1c, q2c;
char *buffer = malloc( BUFSIZE );
FILE *fIn, *fOut;
q0.x = q0.y = 0.0;
if( argc != 3 )
{
fprintf( stderr,
"Usage: %1 nmraturnoutdata paramfile\n\n"
"The data file is read line by line and turnout defimitions\n"
"are created in the param file.\n\n",
argv[ 0 ] );
exit( 1 );
}
fIn = fopen( argv[ 1 ], "r" );
if( !fIn ) {
fprintf( stderr, "Could not open the definition %s\n", argv[ 1 ] );
exit( 1 );
}
fOut = fopen( argv[ 2 ], "w" );
if( !fOut ) {
fprintf( stderr, "Could not create the structures in %s\n", argv[ 2 ] );
exit( 1 );
}
if( fgets( buffer, BUFSIZE, fIn ))
{
printf( "Creating %s\n", buffer + strlen("CONTENTS " ) );
fputs( buffer, fOut );
}
while(fgets(buffer, BUFSIZE, fIn ))
{
if( buffer[ 0 ] == '#' ) {
fputs( buffer, fOut );
continue;
}
scale = strtok( buffer, DELIMITER );
desc = strtok( NULL, DELIMITER );
tg = atof(strtok( NULL, DELIMITER ));
q1.x = getval(strtok( NULL, DELIMITER ));
q1.y = getval(strtok( NULL, DELIMITER ));
pr = getval(strtok( NULL, DELIMITER ));
l = getval(strtok( NULL, DELIMITER ));
crr = getval(strtok( NULL, DELIMITER ));
fa = getval(strtok( NULL, DELIMITER ));
tl = getval(strtok( NULL, DELIMITER ));
hl = getval(strtok( NULL, DELIMITER ));
t = floor(fa);
fa = t + (fa-t)/60*100;
q2.x = l-tl;
q2.y = tg-tl*TAN(fa);
q3.x = l+hl;
q3.y = tg+hl*SIN(fa);
computeCurve( q0, q1, -pr, &q1c, &a10, &a11 );
computeCurve( q1, q2, -crr, &q2c, &a20, &a21 );
fprintf( fOut, "#NMRA-Std TO %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %0.3f\n",
q1.x, q1.y, pr, l, crr, fa, tl, hl );
fprintf( fOut, "TURNOUT %s \"NMRA %s\t#%s Right\t%sR\"\n", scale, scale, desc, desc);
fprintf( fOut, "\tP \"Normal\" 1\n");
fprintf( fOut, "\tP \"Reverse\" 2 3 4\n");
fprintf( fOut, "\tE 0.000000 0.000000 270.000000\n");
fprintf( fOut, "\tE %0.6f 0.000000 90.000000\n", l+hl);
fprintf( fOut, "\tE %0.6f %0.6f %0.6f\n", q3.x, -q3.y, 90.0+fa);
fprintf( fOut, "\tS 0 0 0.000000 0.000000 %0.6f 0.000000\n", l+hl);
fprintf( fOut, "\tC 0 0 %0.6f %0.6f %0.6f %0.6f %0.6f\n", pr, q1c.x, -q1c.y, normalizeAngle(180-a10-a11), a11 );
fprintf( fOut, "\tC 0 0 %0.6f %0.6f %0.6f %0.6f %0.6f\n", crr, q2c.x, -q2c.y, normalizeAngle(180-a20-a21), a21 );
fprintf( fOut, "\tS 0 0 %0.6f %0.6f %0.6f %0.6f\n", q2.x, -q2.y, q3.x, -q3.y );
fprintf( fOut, "\tEND\n");
fprintf( fOut, "TURNOUT %s \"NMRA %s\t#%s Left\t%sL\"\n", scale, scale, desc, desc);
fprintf( fOut, "\tP \"Normal\" 1\n");
fprintf( fOut, "\tP \"Reverse\" 2 3 4\n");
fprintf( fOut, "\tE 0.000000 0.000000 270.000000\n");
fprintf( fOut, "\tE %0.6f 0.000000 90.000000\n", l+hl);
fprintf( fOut, "\tE %0.6f %0.6f %0.6f\n", q3.x, q3.y, 90.0-fa);
fprintf( fOut, "\tS 0 0 0.000000 0.000000 %0.6f 0.000000\n", l+hl);
fprintf( fOut, "\tC 0 0 %0.6f %0.6f %0.6f %0.6f %0.6f\n", -pr, q1c.x, q1c.y, a10, a11 );
fprintf( fOut, "\tC 0 0 %0.6f %0.6f %0.6f %0.6f %0.6f\n", -crr, q2c.x, q2c.y, a20, a21 );
fprintf( fOut, "\tS 0 0 %0.6f %0.6f %0.6f %0.6f\n", q2.x, q2.y, q3.x, q3.y );
fprintf( fOut, "\tEND\n");
}
exit(0);
}
/* Sun rise-set calculation algorithm.
* Algorithm description: http://williams.best.vwh.net/sunrise_sunset_algorithm.htm
*
* Almanac for Computers, 1990
* published by Nautical Almanac Office
* United States Naval Observatory
* Washington, DC 20392
*
* Parameters:
* yd - day of the year (1..365)
* latitude, longitude - Sample: 32.27, 34.85 for Netania israel
* riseset - ESUNRISE or ESUNSET
*
* Returns: UTC hour of the event (a real number).
*/
double sunriseset( int yd, double latitude, double longitude, ERiseSet riseset )
{
// 96 degrees - Calculate Civil twilight time. Used as indication if
// it is (usually) bright enough for outdoor activities without additional lighting.
// 90 degrees 5' - Calculate true Sunrise/Sunset time. Used to check if
// the Sun itself is visible above the horizont in ideal conditions.
const double zenith = 96;
double sinDec, cosDec, cosH;
double H, T, UT;
// Rise / Set
int op = (riseset==ESUNRISE ? 1:-1);
// Convert the longitude to hour value and calculate an approximate time
double lngHour = longitude / 15;
// if rising time is desired:
double t = yd + ((12 - (6*op) - lngHour) / 24);
// Calculate the Sun's mean anomaly
double M = (0.9856 * t) - 3.289;
// Calculate the Sun's true longitude
double L = M + (1.916 * SIN(M)) + (0.020 * SIN(2 * M)) + 282.634;
// Calculate the Sun's right ascension
double RA = ATAN(0.91764 * TAN(L));
// Right ascension value needs to be in the same quadrant as L
double Lquadrant = (floor( L/90)) * 90;
double RAquadrant = (floor(RA/90)) * 90;
RA = RA + (Lquadrant - RAquadrant);
// Right ascension value needs to be converted into hours
RA = RA / 15;
// Calculate the Sun's declination
sinDec = 0.39782 * SIN(L);
cosDec = COS(ASIN(sinDec));
// Calculate the Sun's local hour angle
cosH = (COS(zenith) - (sinDec * SIN(latitude))) / (cosDec * COS(latitude));
if( cosH < -1 || cosH > 1 )
// The sun never rises or sets on this location (on the specified date)
return -1;
// Finish calculating H and convert into hours
H = 180 + (180 - ACOS(cosH))*op;
H = H / 15;
// Calculate local mean time of rising/setting
T = H + RA - (0.06571 * t) - 6.622;
// Adjust back to UTC
UT = T - lngHour;
// UT potentially needs to be adjusted into the range [0,24) by adding/subtracting 24
if (UT < 0) UT += 24;
if (UT >= 24) UT -= 24;
return UT;
}
//
// CastThroughIntersections
//
float Raycaster::CastThroughIntersections(float angle, IntersectionDir dir, int *texIndex, float *texelX)
{
float fx, fy;
float dx, dy;
float distance;
float a, b;
int mapX, mapY;
switch(dir)
{
case ID_HORIZONTAL:
if(UP(angle))
{
fy = -(posY - (int)posY);
dy = -1;
}
else
{
fy = (int)posY + 1 - posY;
dy = 1;
}
fx = (float)(ABS(fy)) / (float)(TAN(angle));
dx = (float)(ABS(dy)) / (float)(TAN(angle));
fx = ABS(fx);
dx = ABS(dx);
if(LEFT(angle))
{
dx = -dx;
fx = -fx;
}
fx = posX + fx;
fy = posY + fy;
break;
case ID_VERTICAL:
if(LEFT(angle))
{
fx = -(posX - (int)posX);
dx = -1;
}
else
{
fx = (int)posX + 1 - posX;
dx = 1;
}
fy = (float)(TAN(angle)) * (float)(ABS(fx));
dy = (float)(TAN(angle)) * (float)(ABS(dx));
fy = ABS(fy);
dy = ABS(dy);
if(UP(angle))
{
fy = -fy;
dy = -dy;
}
fx = posX + fx;
fy = posY + fy;
break;
}
while(true)
{
mapY = (int)fy;
mapX = (int)fx;
if(dy == -1 && dir == ID_HORIZONTAL)
mapY -= 1;
else if(dx == -1 && dir == ID_VERTICAL)
mapX -= 1;
if(mapX < 0 || mapY < 0 || mapX >= mapW || mapY >= mapH)
break;
else if(map[mapY][mapX] > 0 && map[mapY][mapX] != DOOR_INDEX && map[mapY][mapX] != LOCKED_DOOR_INDEX)
{
hit:
if(dir == ID_HORIZONTAL)
*texelX = fx - (float)mapX;
else
*texelX = fy - (float)mapY;
*texIndex = map[mapY][mapX] - 1;
break;
}
else if(map[mapY][mapX] == DOOR_INDEX || map[mapY][mapX] == LOCKED_DOOR_INDEX)
{
Door *door = GetDoorAt(mapX, mapY);
if(door->GetOpening() || door->GetClosing())
{
float xval;
if(dir == ID_HORIZONTAL)
xval = fx - (float)mapX;
else
xval = fy - (float)mapY;
if(door->GetOpenedWidth() < xval)
goto hit;
}
else if(!door->IsOpen())
goto hit;
}
fx += dx;
fy += dy;
}
a = ABS((fy - posY));
b = ABS((fx - posX));
distance = sqrt(a*a+b*b);
return distance;
}
//
// Raycaster
// Constructor
//
Raycaster::Raycaster(RaycasterSetup *setup)
{
int i;
posX = 0.0f;
posY = 0.0f;
alpha = 0.0f;
lookDir.x = 1.0f;
lookDir.y = 0.0f;
camPlane.x = 0.0f;
camPlane.y = COS(FOV);
target = setup->target;
lowQuality = setup->lowQuality;
badQuality = setup->badQuality;
numTextures = setup->numTextures;
for(i=0; i<numTextures; i++)
{
textures[i][0] = setup->textures[i][0];
textures[i][1] = setup->textures[i][1];
}
LoadMap(setup->mapFilename);
sprites = setup->sprites;
roofColor = framework->GetColor(50, 50, 200);
floorColor = framework->GetColor(50, 50, 50);
#ifdef GEKKO
if(!lowQuality)
{
skyImg = framework->LoadImage("images/sky.bmp", false);
roofImg = framework->LoadImage("images/roof.bmp", false);
}
#else
if(!lowQuality)
{
skyImg = framework->LoadImage("images\\sky.bmp", false);
roofImg = framework->LoadImage("images\\roof.bmp", false);
}
#endif
else
{
skyImg = framework->LoadImage("images\\lowQuality\\sky.bmp", false);
roofImg = framework->LoadImage("images\\lowQuality\\roof.bmp", false);
}
upperRect.x = upperRect.y = lowerRect.x = 0;
upperRect.w = lowerRect.w = target->w;
upperRect.h = lowerRect.h = target->w / 2;
lowerRect.y = target->h / 2;
zBuffer = new float[target->w];
pplaneDist = (float)(target->w / 2) / TAN(FOV / 2);
crosshairRect = setup->crosshairRect;
CORR_ANGLE(alpha);
}
//=====================================================
///Execute the skill. This is the main part of the skill, where you tell the
///robot how to perform the skill.
void CutGoalSkill::execute()
{
///If not active, dont do anything!
if(!initialized)
{
return;
}
else
{
//grab the ball location
if(!presetBall){
ball = strategy->getCurrentRoboCupFrame()->getDefensiveBallLocation();
}
///Calculate the angle bisector of the area we want to cover
float theta;
float theta1=angleBetween(sp->field.OUR_GOAL_LINE,point1,ball.getX(),ball.getY());
float theta2=angleBetween(sp->field.OUR_GOAL_LINE,point2,ball.getX(),ball.getY());
theta=(theta1+theta2)/2.0f;
theta=normalizeAngle(theta+PI);
float halfAngle=ABS(angleDifference(theta1,theta2)/2.0f);
//calculate midpoint to extend from
Pair midpoint;
midpoint.setY((sp->field.OUR_GOAL_LINE-ball.getX()) * TAN(theta) + ball.getY());
midpoint.setX(sp->field.OUR_GOAL_LINE);
/*debugging helpful stuff
GUI_Record.debuggingInfo.setDebugPoint(robotID,6,midpoint);
GUI_Record.debuggingInfo.setDebugPoint(robotID,7,sp->field.OUR_GOAL_LINE,point1);
GUI_Record.debuggingInfo.setDebugPoint(robotID,8,sp->field.OUR_GOAL_LINE,point2);
Pair ang1(ball.getX()+.2f,ball.getY());
Pair ang2(ball.getX()+.2f,ball.getY());
rotateAboutPoint(ang1,ball,theta1,ang1);
rotateAboutPoint(ang2,ball,theta2,ang2);
GUI_Record.debuggingInfo.setDebugPoint(robotID,3,ang1);
GUI_Record.debuggingInfo.setDebugPoint(robotID,4,ang2);
Pair t(ball.getX()+.2f,ball.getY());
rotateAboutPoint(t,ball,theta,t);
GUI_Record.debuggingInfo.setDebugPoint(robotID,5,t);
*/
// The ideal point we want the robot to be in this circumstances
Pair dest;
float distance = sp->general.PLAYER_RADIUS / SIN(ABS(halfAngle)) ;
//char msg[80]; sprintf(msg,"dist: %5.2f",distance);GUI_Record.debuggingInfo.addDebugMessage(msg);
extendPoint(midpoint,ball,-distance,dest);
// We have to check if the destination point is between the Upper and lower limit
// float slope = (midpoint.getY() - ball.getY()) / (midpoint.getX() - ball.getY()) ;
// If it is above the limit
if(dest.getX() > UPPER_X){
extrapolateForY(midpoint,ball,UPPER_X,dest);
}
// If it is below the limit
if(dest.getX() < LOWER_X){
extrapolateForY(midpoint,ball,LOWER_X,dest);
}
command->setControl(OMNI_NORMAL);
command->setPos(dest);
command->setRotation(angleBetween(getLocation(robotID, *currentVisionData, *sp),
ball));
strategy->getCurrentFrame()->setMessage(robotID,"Covering Goal");
}
}
static void trv_ray_xcaster23(FAR struct trv_raycast_s *result)
{
struct trv_rect_list_s *list; /* Points to the current X plane rectangle */
struct trv_rect_data_s *rect; /* Points to the rectangle data */
trv_coord_t relx; /* Relative position of the X plane */
trv_coord_t absy; /* Absolute Y position at relx given yaw */
trv_coord_t absz; /* Absolute Z position at relx given pitch */
trv_coord_t lastrelx1 = -1; /* Last relative X position processed */
trv_coord_t lastrelx2 = -1; /* Last relative X position processed */
int32_t dydx; /* Rate of change of Y wrt X (double) */
int32_t dzdx; /* Rate of change of Z wrt X (double) */
/* At a view angle of 90 degrees, no intersections with the g_ray_xplanes are
* possible!
*/
if (g_camera.yaw == ANGLE_90)
{
return;
}
/* Pre-calculate the rate of change of Y and Z with respect to X */
/* The negative tangent is equal to the rate of change of Y with respect
* to the X-axis. The tangent is stored at double the "normal" scaling.
*/
dydx = -TAN(g_camera.yaw);
/* Determine the rate of change of the Z with respect to X. dydx is
* "double" precision; the secant is "double" precision. dzdx will be
* retained as "double" precision.
*/
dzdx = qTOd(g_adj_tanpitch * ABS(g_sec_table[g_camera.yaw]));
/* Look at every rectangle lying in the X plane */
/* This logic should be improved at some point so that non-visible planes
* are "pruned" from the list prior to ray casting!
*/
for (list = g_ray_xplane.tail; list; list = list->blink)
{
rect = &list->d;
/* Search for a rectangle which lies "before" the current camera
* position
*/
if (rect->plane < g_camera.x)
{
/* get the X distance to the plane */
relx = g_camera.x - rect->plane;
#if 0
/* g_ray_xplane is an ordered list, if we have already hit something
* closer, then we can abort the casting now.
*/
if (relx > result->xdist)
{
return;
}
#endif
/* Calculate the Y position at this relative X position. We can skip
* this step if we are processing another rectangle at the same relx
* distance.
*/
if (relx != lastrelx1)
{
int32_t deltay; /* Scale == "triple" */
/* The dydx is stored at double the"normal" scaling -- so deltay
* is "triple" precision
*/
deltay = dydx * ((int32_t) relx);
absy = tTOs(deltay) + g_camera.y; /* back to "single" */
lastrelx1 = relx;
}
/* Check if this Y position intersects the rectangle */
if (absy >= rect->hstart && absy <= rect->hend)
{
/* The Y position lies in the rectangle. Now, calculate the
* Z position at this relative X position. We can skip this
* step if we are processing another rectangle at the same
* relx distance.
*/
if (relx != lastrelx2)
{
int32_t deltaz; /* Scale == TRIPLE */
/* The dzdx is stored at double the"normal" scaling -- so
* deltaz is "triple" precision
*/
deltaz = dzdx * ((int32_t) relx);
absz = tTOs(deltaz) + g_camera.z; /* Back to single */
lastrelx2 = relx;
}
/* Check if this Z position intersects the rectangle */
if (absz >= rect->vstart && absz <= rect->vend)
{
/* We've got a potential hit, let's see what it is */
/* Check if we just hit an ordinary opaque wall */
if (IS_NORMAL(rect))
{
/* Yes..Save the parameters associated with the normal
* wall hit
*/
result->rect = rect;
result->type = MK_HIT_TYPE(BACK_HIT, X_HIT);
result->xpos = absy;
result->ypos = absz;
result->xdist = relx;
result->ydist = ABS(absy - g_camera.y);
result->zdist = ABS(absz - g_camera.z);
/* Terminate X casting */
return;
}
else if (IS_DOOR(rect))
{
/* Check if the door is in motion. */
if (!IS_MOVING_DOOR(rect))
{
/* Save the parameters associated with the normal
* door hit
*/
result->rect = rect;
result->type = MK_HIT_TYPE(BACK_HIT, X_HIT);
result->xpos = absy;
result->ypos = absz;
result->xdist = relx;
result->ydist = ABS(absy - g_camera.y);
result->zdist = ABS(absz - g_camera.z);
/* Terminate X casting */
return;
}
/* The door is in motion, the Z-position to see if we can
* see under the door
*/
else if (absz > g_opendoor.zbottom)
{
/* Save the parameters associated with the moving
* door hit
*/
result->rect = rect;
result->type = MK_HIT_TYPE(BACK_HIT, X_HIT);
result->xpos = absy;
result->ypos = absz - g_opendoor.zdist;
result->xdist = relx;
result->ydist = ABS(absy - g_camera.y);
result->zdist = ABS(absz - g_camera.z);
/* Terminate X casting */
return;
}
}
/* Otherwise, it must be a transparent wall. We'll need to
* make our decision based upon the pixel that we hit
*/
/* Check if the pixel at this location is visible */
else if (GET_BACK_PIXEL(rect, absy, absz) != INVISIBLE_PIXEL)
{
/* Its visible, save the parameters associated with the
* transparent wall hit
*/
result->rect = rect;
result->type = MK_HIT_TYPE(BACK_HIT, X_HIT);
result->xpos = absy;
result->ypos = absz;
result->xdist = relx;
result->ydist = ABS(absy - g_camera.y);
result->zdist = ABS(absz - g_camera.z);
/* Terminate X casting */
return;
}
}
}
}
}
}
void trv_raycast(int16_t pitch, int16_t yaw, int16_t screenyaw,
FAR struct trv_raycast_s *result)
{
/* Set the camera pitch and yaw angles for this cast */
g_camera.pitch = pitch;
g_camera.yaw = yaw;
/* Initialize the result structure, assuming that there will be no hit */
result->rect = NULL;
result->type = NO_HIT;
result->xpos = 0;
result->ypos = 0;
result->xdist = TRV_INFINITY;
result->ydist = TRV_INFINITY;
result->zdist = TRV_INFINITY;
/* Prepare for X and Y ray casts. These casts will need the adjusted tangent
* of the pitch angle in order to correct for "fish eye" distortion. This
* correction consists of multiplying by the cosine of the relative screen
* yaw position. The tangent is double precision, the cosine is double
* precision, the result will be retained as double precision.
*/
screenyaw = ABS(screenyaw);
#if ENABLE_VIEW_CORRECTION
g_adj_tanpitch = qTOd(TAN(pitch) * ((int32_t) g_cos_table[screenyaw]));
#else
g_adj_tanpitch = TAN(pitch);
#endif
/* Perform X & Y raycasting based on the quadrant of the yaw angle */
if (g_camera.yaw < ANGLE_90)
{
trv_ray_xcaster14(result);
trv_ray_ycaster12(result);
}
else if (g_camera.yaw < ANGLE_180)
{
trv_ray_xcaster23(result);
trv_ray_ycaster12(result);
}
else if (g_camera.yaw < ANGLE_270)
{
trv_ray_xcaster23(result);
trv_ray_ycaster34(result);
}
else
{
trv_ray_xcaster14(result);
trv_ray_ycaster34(result);
}
/* Perform Z ray casting based upon if we are looking up or down */
if (g_camera.pitch < ANGLE_90)
{
/* Get the adjusted cotangent of the pitch angle which is used to correct
* for the "fish eye" distortion. This correction consists of
* multiplying by the inverted cosine of the relative screen yaw
* position. The cotangent is double precision, the secant is double
* precision, the result will be retained as double precision.
*/
#if ENABLE_VIEW_CORRECTION
g_adj_cotpitch = qTOd(g_cot_table(pitch) * g_sec_table[screenyaw]);
#else
g_adj_cotpitch = g_cot_table(pitch);
#endif
trv_ray_zcasteru(result);
}
else
{
/* Get the adjusted cotangent of the pitch angle which is used to correct
* for the "fish eye" distortion. This correction consists of
* multiplying by the inverted cosine of the relative screen yaw
* position. The cotangent is double precision, the secant is double
* precision, the result will be retained as double precision.
*/
#if ENABLE_VIEW_CORRECTION
g_adj_cotpitch =
qTOd(g_cot_table(ANGLE_360 - pitch) * g_sec_table[screenyaw]);
#else
g_adj_cotpitch = g_cot_table(ANGLE_360 - pitch);
#endif
trv_ray_zcasterl(result);
}
}
