double dynamics_mul::energy_change(Node_key nid, Vec2 new_pos) {
double dE = 0.0;
double E0 = intensity_energy(nid, s_dsc->get_pos(nid));
double E1 = intensity_energy(nid, new_pos);
double L0 = curve_length(nid, s_dsc->get_pos(nid));
double L1 = curve_length(nid, new_pos);
dE = (L1 - L0)*g_param.alpha + (E1 - E0)*g_param.beta;
return dE;
}
void dynamics_mul::debug_optimum_dt(){
alpha_map_.clear();
for (auto ni = s_dsc->vertices_begin(); ni != s_dsc->vertices_end(); ni++) {
if (s_dsc->is_interface(*ni)) {
Vec2 s = (s_dsc->get_node_internal_force(*ni)
+ s_dsc->get_node_external_force(*ni));
// 1. Find furthest movement
double alpha_max = furthest_move(*ni, s);
// 2. Compute energy change
double delta_E = energy_change(*ni, s_dsc->get_pos(*ni) + s * alpha_max);
// 3. Compute partial derivative of E wrt to alpha
double ll = curve_length(*ni, s_dsc->get_pos(*ni));
double grad_E_a_2 = s.length() * s.length() / ll;
// 4. Optimal alpha
double alpha_g = 1.0/2.0*(2.0*delta_E - alpha_max*grad_E_a_2)/(delta_E - alpha_max*grad_E_a_2);
std::cout << " " << alpha_g << std::endl;
}
}
}
/*
* void generate_curve( struct sphere ball);
*
* generates a new bezier curve based on a previous one
*/
void generate_curve( struct sphere *ball) {
// store previous position
ball->previous_pos = ball->pos;
ball->interval = 0.0;
ball->p1.x = ball->p4.x;
ball->p2.x = ( ball->p4.x - ball->p3.x ) + ball->p4.x;
//ball->p3.x = ranged_random_value();
//ball->p4.x = ranged_random_value();
ball->p2.y = ( ball->p4.y - ball->p3.y ) + ball->p4.y;
ball->p1.y = ball->p4.y;
//ball->p3.y = ranged_random_value();
//ball->p4.y = ranged_random_value();
ball->p3 = new_curve_point(ball->p2);
//printf(" x:%f y:%f\n",ball->p3.x,ball->p3.y);
ball->p4 = new_curve_point(ball->p3);
//printf(" x:%f y:%f\n",ball->p4.x,ball->p4.y);
ball->curve_length = curve_length( ball );
ball->start_time = (double) clock();
ball->curve_time = ball->curve_length / ball->velocity;
//return ball;
}
double dynamics_mul::star_energy(Node_key nid, Vec2 new_pos){
double E1 = intensity_energy(nid, new_pos);
double L1 = curve_length(nid, new_pos);
// return L1*g_param.alpha + E1*g_param.beta;
return E1;
}
/* Compute parametrization info. That is, for each part of the cairo path, tags it with
* its length. */
static parametrization_t* parametrize_path(cairo_path_t* path) {
parametrization_t* parametrization = 0;
cairo_path_data_t* data = 0;
cairo_path_data_t last_move_to;
cairo_path_data_t current_point;
int i;
current_point.point.x = 0.0;
current_point.point.y = 0.0;
parametrization = (parametrization_t*)malloc(path->num_data * sizeof(parametrization[0]));
for(i = 0; i < path->num_data; i += path->data[i].header.length) {
data = &path->data[i];
parametrization[i] = 0.0;
switch(data->header.type) {
case CAIRO_PATH_MOVE_TO:
last_move_to = data[1];
current_point = data[1];
break;
case CAIRO_PATH_CLOSE_PATH:
/* Make it look like it's a line_to to last_move_to. */
data = (&last_move_to) - 1;
case CAIRO_PATH_LINE_TO:
parametrization[i] = two_points_distance(¤t_point, &data[1]);
current_point = data[1];
break;
case CAIRO_PATH_CURVE_TO:
parametrization[i] = curve_length(
current_point.point.x, current_point.point.x,
data[1].point.x, data[1].point.y,
data[2].point.x, data[2].point.y,
data[3].point.x, data[3].point.y
);
current_point = data[3];
break;
}
}
return parametrization;
}
/* Compute parametrization info. That is, for each part of the
* cairo path, tags it with its length.
*
* Free returned value with g_free().
*/
static parametrization_t *
parametrize_path (cairo_path_t *path)
{
int i;
cairo_path_data_t *data, last_move_to, current_point;
parametrization_t *parametrization;
parametrization = g_malloc (path->num_data * sizeof (parametrization[0]));
for (i=0; i < path->num_data; i += path->data[i].header.length) {
data = &path->data[i];
parametrization[i] = 0.0;
switch (data->header.type) {
case CAIRO_PATH_MOVE_TO:
last_move_to = data[1];
current_point = data[1];
break;
case CAIRO_PATH_CLOSE_PATH:
/* Make it look like it's a line_to to last_move_to */
data = (&last_move_to) - 1;
/* fall through */
case CAIRO_PATH_LINE_TO:
parametrization[i] = two_points_distance (¤t_point, &data[1]);
current_point = data[1];
break;
case CAIRO_PATH_CURVE_TO:
/* naive curve-length, treating bezier as three line segments:
parametrization[i] = two_points_distance (¤t_point, &data[1])
+ two_points_distance (&data[1], &data[2])
+ two_points_distance (&data[2], &data[3]);
*/
parametrization[i] = curve_length (current_point.point.x, current_point.point.x,
data[1].point.x, data[1].point.y,
data[2].point.x, data[2].point.y,
data[3].point.x, data[3].point.y);
current_point = data[3];
break;
default:
g_assert_not_reached ();
}
}
return parametrization;
}
/*
* struct sphere generate_sphere();
*
* return a ball
*/
struct sphere generate_sphere() {
struct sphere ball;
do{
// RADIUS MUST BE BEFORE RANDOM POINTS
ball.radius = random_radius();
// gets 4 random points for the bezier curve
ball.p1 = random_ranged_point(ball.radius);
ball.p2 = new_curve_point(ball.p1);
ball.p3 = new_curve_point(ball.p2);
ball.p4 = new_curve_point(ball.p3);
// position starts at p1
ball.pos = ball.p1;
ball.previous_pos = {0.0, 0.0};
//printf("generation: %f %f || %f %f\n", ball.previous_pos.x, ball.previous_pos.y, ball.pos.x, ball.pos.y);
// gets a random direction, this might be a wasted step
ball.direction = random_direction(ball.radius);
ball.active = 0;
ball.velocity = random_velocity();
// start on a curved path
ball.path = 1;
// dead variable is set to 0, used to prevent collisions
//ball.dead = 0;
ball.color = random_color();
ball.curve_length = curve_length( &ball );
ball.start_time = (double) clock();
ball.curve_time = ball.curve_length / ball.velocity;
ball.ghost = 0;
}while(collision_detection(ball) == 1 );
return ball;
}
/*
* struct sphere generate_sphere();
*
* return a ball
*/
struct sphere generate_sphere(int rad) {
struct sphere ball;
do{
// RADIUS MUST BE BEFORE RANDOM POINTS
ball.radius = (rad) ? next_ball_radius : random_radius();
// gets 4 random points for the bezier curve
ball.p1 = random_ranged_point(ball.radius);
ball.p2 = new_curve_point(ball.p1);
ball.p3 = new_curve_point(ball.p2);
ball.p4 = new_curve_point(ball.p3);
// position starts at p1
ball.pos = ball.p1;
// 3D
ball.previous_pos = {0.0, 5.0, 0.0};
// gets a random direction, this might be a wasted step
ball.direction = random_direction(ball.radius);
ball.active = 0;
ball.velocity = random_velocity();
// start on a curved path
ball.path = 1;
ball.color = random_color();
ball.curve_length = curve_length( &ball );
ball.start_time = (double) clock();
ball.curve_time = ball.curve_length / ball.velocity;
ball.ghost = 0;
// 3D
}while(collision_detection(ball) == 1 || ball.p1.x == 0.0 || ball.p1.y == 0.0 ||
ball.p1.z == 0.0);
return ball;
}
con_vec Djoefe (situation & s)
{
const char name[] = "Djoefe"; // This is the robot driver's name!
static int init_flag = 1; // cleared by first call
con_vec result; // This is what is returned.
double alpha, vc; // components of result
static double lane = -10000; // an absurd value to show not initialized
double bias, speed, width, to_end;
double speed_next= 0.0;
static double fuel_total = 0;
static double fuel_last;
double aftaft_bd, after_bd, bd_now;
// Load the optimization stuff for this track
#ifdef OPTIMIZE
static int optinit = 0;
if(optinit==0)
{
// Reload the population from the data file
const char* optfile = "djoefe.opt";
data.load(optfile,Population,GeneticLength,SetTrackData);
cout << "\nOptimizing for track in " << optfile << endl;
optinit = 1;
}
#endif //OPTIMIZE
if (init_flag == 1)
{ // first time only, copy name:
my_name_is (name); // copy the name string into the host program
init_flag = 0;
result.alpha = result.vc = 0;
return result;
}
// set fuel for qualifications
if (s.starting && s.stage == QUALIFYING)
result.fuel_amount = 20;
// estimate available friction:
useful_friction = 0.975 * my_friction - 0.2 * s.damage * s.damage / 9e8;
// drive carefully on first laps:
// if(lap_count - s.laps_to_go < 2 && s.position >= 4)
// useful_friction *= .9*my_friction;
// service routine in the host software to handle getting unstuck from
// from crashes and pileups:
#ifndef OPTIMIZE
if (stuck (s.backward, s.v, s.vn, s.to_lft, s.to_rgt, &result.alpha, &result.vc))
return result;
#endif //OPTIMIZE
width = s.to_lft + s.to_rgt; // compute width of track
// This is a little trick so that the car will not try to change lanes
// during the "dragout" at the start of the race. We set "lane" to
// whatever position we have been placed by the host.
if (lane < -9000) // will be true only once
{
fuel_last = s.fuel; // setup fuel counter
#ifndef OPTIMIZE
set_track_data ();
#endif
lane = s.to_lft; // better not to change lanes at the start
slip = BIG_SLIP;
t = get_track_description ();
if( !args.m_iSurface ) // friction model 0:
my_friction = slip / (slip + 2.5);
else // friction model 1:
my_friction = 1.05 * (1.0 - exp (-slip / 2.5));
}
#ifdef OPTIMIZE
if(s.starting)
{
// Figure out a nice estimate of the MPH, or how far we
// got around the track
double mph = optimizedata::mph(lastlaptime,t.length,lastdamage,lastdistance);
if(no_display)
{
cout << " Mph: " << mph
<< endl;
}
// Tell the optimizer what the last MPH was
data.step(mph);
AbortRace = 0;
}
#endif //OPTIMIZE
// Curve control 1
// a) If we are in a curve, go to the inside
// b) If we are exiting the curve into a straight then "slide" out of the curve
// Sharp left
if (s.cur_rad > 0.0 && s.cur_rad < straight_radius)
{
if (is_nex_straight (s) && s.to_end <= s.cur_len * (1 - out_of_curve))
{
lane = width - dist_from_outside;
}
else
{
lane = dist_from_inside;
}
}
// Sharp right
else if (s.cur_rad < 0.0 && s.cur_rad > -straight_radius)
{
if (is_nex_straight (s) && s.to_end <= s.cur_len * (1 - out_of_curve))
{
lane = dist_from_outside;
}
else
{
lane = width - dist_from_inside;
}
}
// set the bias:
// Bias is an additive term in the steering servo, so that the servo
// doesn't have to "hunt" much for the correct alpha value. It is an
// estimate of the alpha value that would be found by the servo if there
// was plenty of settling time. It is zero for straightaways.
// Also, for convenience, we call the corn_spd() function here. On
// the straightaway, we call it to find out the correct speed for the
// corner ahead, using s.nex_rad for the radius. In the curve we of
// course use the radius of the curve we are in. But also, we call it
// for the next segment, to find out our target speed for the end of
// the current segment, which we call speed_next.
if (s.cur_rad == 0.0)
{
bias = 0.0;
if (!is_nex_straight (s))
speed = curvespeed (s, fabs(s.nex_rad) + width * predict_width);
else
speed = 250.0;
}
else
{
if (is_nex_straight(s))
speed_next = 250.0;
else
speed_next = curvespeed (s, fabs (s.nex_rad) + width * predict_width);
if(!is_curr_straight(s))
speed = curvespeed (s, fabs (s.cur_rad) + width * predict_width);
else
speed = 250;
bias = (s.v * s.v / (speed * speed)) * atan (BIG_SLIP / speed);
if (s.cur_rad < 0.0) // bias must be negative for right turn
bias = -bias;
}
// set alpha: (This line is the complete steering servo.)
alpha = steer_gain * (s.to_lft - lane) / width - damp_gain * s.vn / s.v + bias;
// set vc: When nearing end of straight, change "lane" for the turn, also.
if (s.cur_rad == 0.0)
{ // If we are on a straightaway,
// if we are far from the end,
if (s.to_end * ignore_slowdown > find_bd (s, speed))
{
vc = s.v + 50.0; // pedal to the metal!
}
else
{ // otherwise, adjust speed for the coming turn:
if (s.v > 1.02 * speed) // if we're 2% too fast,
vc = brake_ratio * s.v; // brake hard.
else if (s.v < .98 * speed) // if we're 2% too slow,
vc = 1.1 * speed; // accelerate hard. 1.1
else // if we are very close to speed,
vc = .5 * (s.v + speed); // approach the speed gently.
}
// Start diving into the corner
if(s.to_end < (start_dive * s.v * width))
{
if (s.nex_rad > 0.0) // left
lane = dist_from_inside;
else
lane = width - dist_from_inside;
}
else
{
if (!is_nex_straight(s) && ((s.cur_len * dive_setup_dist) >= s.to_end))
{
if (s.nex_rad > 0.0)
lane = width - dist_for_dive;
else if (s.nex_rad < 0.0)
lane = dist_for_dive;
}
}
}
else
{
// This is when we are in a curve: (seek correct speed)
// calculate vc to maintain speed in corner
speed = curvespeed (s, fabs (s.cur_rad) + width * predict_width);
vc = .5 * (s.v + speed);
// calculate distance to end of curve
if (s.cur_rad > 0.0)
to_end = s.to_end * (s.cur_rad + width * predict_width);
else
to_end = -s.to_end * (s.cur_rad - width * predict_width);
// compute required braking distance and compare:
if (to_end <= find_bd (s, speed_next))
{
vc = s.v - brake_curve_slip;
}
if (is_nex_straight (s) && s.to_end <= s.cur_len * (1 - out_of_curve))
{
vc = s.v * 1.1;
}
}
// New speed limiter if curve after next is tricky
// calculate the braking distance we will need to slow down from the
// speed we now have to the speed in the segment after the next
aftaft_bd = find_bd(s, crvspeed(s.aftaft_rad));
after_bd = find_bd(s, crvspeed(s.after_rad));
if (aftaft_bd > (curve_length(s.nex_len, s.nex_rad)
+ curve_length(s.after_len, s.after_rad)))
{
bd_now = aftaft_bd - (curve_length(s.nex_len, s.nex_rad) +
curve_length(s.after_len, s.after_rad));
if (bd_now > curve_length(s.to_end, s.cur_rad))
{
vc = s.v * brake_ratio;
}
}
// else here ? not sure...
if (after_bd > (curve_length(s.nex_len, s.nex_rad)))
{
// We will have to start braking in this segment.
// When = Total - lenght of next curve
bd_now = after_bd - curve_length(s.nex_len, s.nex_rad);
if (bd_now > curve_length(s.to_end, s.cur_rad))
{
vc = s.v * brake_ratio;
}
}
#ifndef BFOPT
// Gotta love this passing code... ;-)
if (s.dead_ahead) // Change the lane a little if someone's
if (s.to_lft > s.to_rgt) // in your way.
lane -= DELTA_LANE; // lane must be a static variable
else
lane += DELTA_LANE;
if (s.lap_flag)
{
if (s.fuel > fuel_last)
{
// we refueled...use previous as estimate
fuel_total += fuel_total / (s.laps_done - 1);
}
else
{
fuel_total += fuel_last - s.fuel;
}
fuel_last = s.fuel;
}
result.request_pit = 0;
if (s.stage != QUALIFYING && (s.damage > 20000) && (s.laps_to_go > 5))
{
result.request_pit = 1;
result.repair_amount = max(s.damage, (unsigned)s.laps_to_go * 1000);
result.fuel_amount = max(0,((fuel_total / s.laps_done) * (s.laps_to_go + 1)) - s.fuel);
}
if (s.stage != QUALIFYING && s.fuel < (fuel_total / (s.laps_done - 1)) && s.laps_done > 1)
{
result.request_pit = 1;
result.fuel_amount = (fuel_total / s.laps_done) * (s.laps_to_go + 1);
result.repair_amount = (int)result.fuel_amount * 10;
}
#endif
result.alpha = alpha;
result.vc = vc;
traction_control(s, result);
#ifdef OPTIMIZE // Pitting during optimizing is done differently
// Keep track of the data from the last lap
lastlaptime = s.lap_time;
lastdamage = s.damage;
lastdistance = s.distance;
// result.fuel_amount = 150;
result.request_pit = 0;
// If we are damaged or are too close to the edge, abort the
// race by deliberate crashing
/* if((s.damage)||
(s.to_lft<EdgeLimit)||
(s.to_rgt<EdgeLimit))
AbortRace = 1;*/
if(AbortRace)
{
result.vc = 250.;
result.alpha = 0.;
}
#endif //OPTIMIZE
#ifdef SKIDMARKS
extern int skidmarks;
skidmarks = 1;
extern int designated;
designated = 0;
#endif
return result;
}
