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); } }