// RAMDU.CPP for fourmile.trk #include #include #include #include "car.h" #include #define min(x,y) (((x)>(y))?(y):(x)) #define max(x,y) (((x)<(y))?(y):(x)) #define fabs(x) (((x)<0.0)?(-1.0*(x)):(x)) #define sign_turn(rad) (((rad) < 0.0) ? (-1.0) : (1.0)) #define can_decelerate(dist, inv_spd, spd, corn_spd) ((decel_const*(dist)*20.0*(inv_spd)) > ((spd) - (corn_spd))) #define PI_34 2.3561944 #define PI_2 1.57079632679489661923 #define PI_4 0.7853981 #define PI_1_2 0.6366197 #define PI_1 0.3183098 #define SPEED_CORN 2.0 #define SPEED_CORN2 3.0 #define DIST_CORN 180.0 #define SHORT_CORN (PI*0.5) #define SHORT_CORN_POWER 3.0 #define ACCEL_CORN (PI/3.0) #define CORN_POWER 0.5 #define steer_gain .18 extern const double CARWID; double r_fusion(double value1, double value2, double length, double to_end); double r_adj_alpha(double alpha, double speed, double corn_speed); double r_get_alpha(double to_inside, double to_outside, double width); double r_accel_corn(double corn_spd, double len_rad, double len_ft, double nex_rad, double after_rad); static double corn_con = 5.872933 ; static double decel_const = 2.657731 ; static double dist_next_width = 1.287285 ; static double dist_next_inside = 0.810238 ; static double corn1 = 1.123118 ; static double corn2 = 1.092294 ; static double corn3 = 1.454545 ; static double corn4 = 0.673542 ; static double corn5 = 0.930702 ; static double corn6 = 1.194024 ; static double fusion_pow = 2.538256 ; static double adj_alpha_pow = 1.159872 ; static double short_corn = 2.419606 ; static double short_corn_power = 4.01663 ; static double speed_corn = 1.236311 ; static double speed_corn2 = 0.248649 ; static double dist_corn = 600 ; static double accel_corn = 0.516688 ; static double drift_corn = 0.725493 ; static double drift_inside = 0.003791 ; static double drift_inside_mult = 1.483808 ; static double corn_mult = 5.053316 ; static double corn_inside_mult = 1.391028 ; static double corn_outside_mult = 0.188757 ; con_vec Ramdu(situation& s) { const char name[] = "Ramdu"; static int init_flag = 1; con_vec result; double alpha, vc; double Accel_vc; double width; // track width, feet double cur_vel; // current velocity double inv_spd; // 1/s.v double cur_length; // current length in feet double nex_length; // current length in feet double nex_len; // next radius double nex_alpha; // alpha we'll need on the next turn double dist_from_inside; // distance from inside of turn double cur_dist_from_inside; // distance from inside of turn double cur_dist_from_outside; // distance from inside of turn double nex_dist_from_inside; // distance from inside of turn double nex_dist_from_outside; // distance from inside of turn double dist_to_end; // distance from inside of turn double dist_start_next; // distance from inside of turn double nex_corn_spd; // speed for next corner double tmp_corn_spd; // speed for next corner double drift_to; // speed for next corner static double drift_to_mult; // speed for next corner static double drift_to_spot; // speed for next corner static double width_mult; int same_turn; const double slow_same_turn = 0.95; const double min_corn_spd = 40.0; const double steer_damp = .48; static double corn_spd = 65.0; static double acc_const = 5000.0; static int max_accel = 0; // FILE *fp; if(init_flag) { my_name_is(name); init_flag = 0; result.alpha = result.vc = 0; return result; } if(stuck(s.backward, s.v, s.vn, s.to_lft, s.to_rgt, &result.alpha, &result.vc)) return result; width = s.to_lft + s.to_rgt; // find width of track if (s.starting) { drift_to_mult = 1.0 / (width * 10.0); width_mult = width/120.0; width_mult *= width_mult; // Modifiers to try and handle most tracks decel_const += (125.0 - width)/125.0; corn_con += (sqrt(width/125.0) - 1.0); /* fp = fopen("gene.txt", "rt"); fscanf(fp, "%lf %lf %lf %lf ", &corn_con, &decel_const, &dist_next_width, &dist_next_inside); fscanf(fp, "%lf %lf %lf %lf %lf %lf", &corn1, &corn2, &corn3, &corn4, &corn5, &corn6); fscanf(fp, "%lf %lf %lf %lf ", &fusion_pow, &adj_alpha_pow, &short_corn, &short_corn_power); fscanf(fp, "%lf %lf %lf %lf ", &speed_corn, &speed_corn2, &dist_corn, &accel_corn); fscanf(fp, "%lf %lf %lf ", &drift_corn, &drift_inside, &drift_inside_mult); fscanf(fp, "%lf %lf %lf ", &corn_mult, &corn_inside_mult, &corn_outside_mult); fclose(fp); */ } /*************************************/ /* First calculate some basic values */ /*************************************/ cur_vel = sqrt(s.v * s.v + s.vn * s.vn); if (((s.cur_rad < 0.0) && (s.nex_rad < 0.0)) || ((s.cur_rad > 0.0) && (s.nex_rad > 0.0))) same_turn = 1; else same_turn = 0; if ((s.nex_rad > 0.0) || (((s.nex_rad == 0.0) || (fabs(s.nex_rad) > 2500.0)) && (s.after_rad > 0.0))) { nex_dist_from_inside = s.to_lft; nex_dist_from_outside = s.to_rgt; } else if ((s.nex_rad < 0.0) || (((s.nex_rad == 0.0) || (fabs(s.nex_rad) > 2500.0)) && (s.after_rad < 0.0))) { nex_dist_from_inside = s.to_rgt; nex_dist_from_outside = s.to_lft; } if (s.cur_rad > 0.0) { cur_dist_from_inside = s.to_lft; cur_dist_from_outside = s.to_rgt; } else if (s.cur_rad < 0.0) { cur_dist_from_inside = s.to_rgt; cur_dist_from_outside = s.to_lft; } else { cur_dist_from_inside = nex_dist_from_inside; cur_dist_from_outside = nex_dist_from_outside; } //cur_length = (fabs(s.cur_rad) + width*0.5) * s.cur_len; //nex_length = (fabs(s.nex_rad) + width*0.5) * s.nex_len; cur_length = (fabs(s.cur_rad) + cur_dist_from_inside) * s.cur_len; nex_length = (fabs(s.nex_rad) + nex_dist_from_inside) * s.nex_len; /*****************************************************************/ /* Calculate the cornering speeds for this, and the next corners */ /*****************************************************************/ corn_spd = 350.0; nex_corn_spd = 350.0; if ((s.cur_rad == 0.0) || (fabs(s.cur_rad) > 2500.0)) { if (fabs(s.cur_rad) > 2500.0) dist_to_end = s.to_end * (fabs(s.cur_rad) + (cur_dist_from_inside)); else dist_to_end = s.to_end; if (s.nex_rad != 0.0) { corn_spd = max(min_corn_spd, corn_con * pow(fabs(s.nex_rad), CORN_POWER)); nex_len = s.nex_len; if (((s.nex_rad < 0.0) && (s.after_rad < 0.0)) || ((s.nex_rad > 0.0) && (s.after_rad > 0.0))) nex_len *= 2.0; corn_spd = r_accel_corn(corn_spd, nex_len, nex_length, s.nex_rad, s.after_rad); } if ((s.after_rad != 0.0) && (s.after_rad < 2500.0)) { nex_corn_spd = max(min_corn_spd, corn_con * pow(fabs(s.after_rad), CORN_POWER)); } } else { dist_to_end = s.to_end * (fabs(s.cur_rad) + (cur_dist_from_inside)); corn_spd = max(min_corn_spd, corn_con * pow(fabs(s.cur_rad), CORN_POWER)); corn_spd = r_accel_corn(corn_spd, s.cur_len, cur_length, s.cur_rad, s.nex_rad); if (same_turn) corn_spd *= slow_same_turn; if ((s.nex_rad != 0.0) && (s.nex_rad < 2500.0)) { nex_corn_spd = max(min_corn_spd, corn_con * pow(fabs(s.nex_rad), CORN_POWER)); nex_corn_spd = r_accel_corn(nex_corn_spd, s.nex_len, nex_length, s.nex_rad, s.after_rad); } } // This is to make sure we don't divide by zero inv_spd = (s.v < 1e-5 && s.v > -1e-5) ? (1e5) : 1.0/s.v; /******************************************************************/ /* Calculate whether we need to change speed for the next segment */ /******************************************************************/ Accel_vc = s.v + acc_const * inv_spd; if((s.cur_rad == 0.0) || (fabs(s.cur_rad) > 2500.0)) // If we are on a straightaway, if((decel_const*dist_to_end*20.0*inv_spd) > (s.v - corn_spd)) { // decel_const is avg deceleration, v*0.05 is avg feet we travel per turn vc = Accel_vc; // keep accelerating near full power } else { // otherwise, vc = ((Accel_vc accel_corn) vc = corn_spd; // First half of corner, keep corn_spd else vc = nex_corn_spd; if (((s.nex_rad == 0.0) || (fabs(s.nex_rad) > 2500.0)) && (s.after_rad != 0)) { // The next straight is so short, let's decelerate to the corner after that. tmp_corn_spd = max(min_corn_spd, corn_con * pow(fabs(s.after_rad), CORN_POWER)); if((decel_const*(dist_to_end+nex_length)*20.0*inv_spd) < (s.v - tmp_corn_spd)) { vc = tmp_corn_spd; } } } else { if(((decel_const*dist_to_end*20.0*inv_spd) > (s.v - nex_corn_spd))){ // 1.5 is avg deceleration, v*0.05 is avg feet we travel per turn vc = corn_spd; // First half of corner, keep corn_spd } else { vc = nex_corn_spd; } } vc = ((Accel_vc 2500.0)) nex_alpha = r_adj_alpha(nex_alpha, cur_vel, corn_spd); else if (nex_corn_spd > corn_spd) nex_alpha = r_adj_alpha(nex_alpha, cur_vel, nex_corn_spd); } if((s.cur_rad == 0.0) || (fabs(s.cur_rad) > 2500.0)) { alpha = 0.0; //dist_start_next = nex_dist_from_inside; dist_start_next = dist_next_width * width + dist_next_inside * nex_dist_from_inside; if (dist_to_end < dist_start_next) { alpha = r_fusion(0.0, nex_alpha, dist_start_next, dist_to_end); } else if (s.v > 120.0){ // drift // Pick our spot for entering the next turn drift_to_spot = width * (drift_inside + drift_inside_mult * s.nex_len * PI_1); // if the next corner is < 90 degrees, drift inside drift_to = (s.nex_len < (drift_corn)) ? (CARWID + (drift_to_spot-CARWID)*(s.nex_len* drift_corn)) : (drift_to_spot); if ((s.nex_rad != 0.0) && (s.nex_rad < 2500.0)) { alpha = sign_turn(s.nex_rad) * (nex_dist_from_inside - drift_to) * drift_to_mult; } } } else { alpha = sign_turn(s.cur_rad) * r_get_alpha(cur_dist_from_inside, cur_dist_from_outside, width); alpha = r_adj_alpha(alpha, cur_vel, corn_spd); if (dist_to_end < 400.0) { //dist_start_next = 100.0; dist_start_next = 75.0; if ((s.cur_rad != 0.0) && (s.cur_len > PI_4)) dist_start_next += 0.3 * (s.cur_len - PI_4) * fabs(s.cur_rad); if (fabs(s.cur_rad) < 70.0) { if ((s.cur_len < PI_2) && ((s.nex_rad == 0) || (fabs(s.nex_rad) > 2500.0))) dist_start_next *= corn1 * (nex_dist_from_inside/width); else if ((s.cur_len > PI_34)) dist_start_next *= corn2 * (nex_dist_from_inside/width); else if ((s.cur_len < PI_34) && (s.cur_len > PI_4)) dist_start_next *= corn3 * (nex_dist_from_inside/width); } else if ((s.cur_len > PI_34) && ((s.nex_rad != 0.0) || (((s.nex_rad == 0.0) || (fabs(s.nex_rad) > 2500.0)) && (nex_length < 150)))) dist_start_next *= corn4 * (nex_dist_from_inside/width); else if ((s.cur_len < PI_34) && ((s.nex_rad != 0.0) || (((s.nex_rad == 0.0) || (fabs(s.nex_rad) > 2500.0)) && (nex_length < 150)))) dist_start_next *= corn5 * (nex_dist_from_inside/width); else if (cur_length < 125.0) dist_start_next *= corn6 * (nex_dist_from_inside/width); dist_start_next *= width_mult; if (dist_to_end < dist_start_next) alpha = r_fusion(alpha, nex_alpha, dist_start_next, dist_to_end); // Scale alpha down to nex_alpha } } alpha -= steer_damp * s.vn * inv_spd; // This is damping, to prevent oscillation // This is a kludge if we're drifting too far outside if (cur_dist_from_outside < CARWID) alpha *= 4.0; result.vc = vc; result.alpha = alpha; return result; } // v; double r_fusion(double value1, double value2, double length, double to_end) { double v1, v2; double value = to_end/length; value = pow(value, fusion_pow); v1 = value1 * value; v2 = value2 * (1.0 - value); return (v1 + v2); } double r_adj_alpha(double alpha, double speed, double corn_speed) { double factor = speed/corn_speed; factor = pow(factor, adj_alpha_pow); return (alpha * factor); } double r_get_alpha(double to_inside, double to_outside, double width) { if(to_outside < 1e-5 && to_outside > -1e-5) to_outside = .01; if (to_inside < (double)CARWID) return(steer_gain * -1.0 * (corn_mult * pow((1.0*CARWID - to_inside) / width, corn_inside_mult) + min(10.0, corn_outside_mult * width/to_outside))); else return(steer_gain * (corn_mult * pow((to_inside - 1.0*CARWID) / width, corn_inside_mult) + min(10.0, corn_outside_mult * width/to_outside))); } double r_accel_corn(double corn_spd, double len_rad, double len_ft, double nex_rad, double after_rad) { if (len_rad < short_corn) { corn_spd += corn_spd * speed_corn * pow((short_corn-len_rad)/short_corn, short_corn_power); if (len_ft < dist_corn) { if (fabs(nex_rad) > 70.0) corn_spd += corn_spd * speed_corn2 * (dist_corn - len_ft)/dist_corn; else corn_spd += corn_spd * (0.5 * (fabs(nex_rad))/70.0) * speed_corn2 * (dist_corn - len_ft)/dist_corn; if ((after_rad != 0.0) && (after_rad < 2500.0) && ((nex_rad > 0.0 && after_rad < 0.0) || (nex_rad < 0.0 && after_rad > 0.0))) corn_spd += corn_spd * 0.5 * (pow((PI_2 - len_rad)/PI_2, 2.0)) * speed_corn2 * (dist_corn - len_ft)/dist_corn; } } return(corn_spd); }