@@ -447,12 +447,12 @@ FrenetPoint PathGenerator::getFrenetPoint(
447
447
// e^(-lambda t_stop) = 1 - (-Vo* lambda)/acc
448
448
// t_stop = (-1/lambda)*ln(1 - (-Vo* lambda)/acc)
449
449
// t_stop = (-1/lambda)*ln(1 + (Vo* lambda)/acc)
450
- auto t_stop = (-1.0 / lambda) * std::log ( 1 + ( v * lambda / a) );
450
+ auto t_stop = (-1.0 / lambda) * std::log1p ( v * lambda / a);
451
451
452
452
// Calculate the distance traveled until stopping
453
453
auto distance_to_reach_zero_speed =
454
454
v * t_stop + a * t_stop * (1.0 / lambda) +
455
- a * (1.0 / std::pow (lambda, 2 )) * ( std::exp (-lambda * t_h) - 1 );
455
+ a * (1.0 / std::pow (lambda, 2 )) * std::expm1 (-lambda * t_h);
456
456
// Output an equivalent constant speed
457
457
return distance_to_reach_zero_speed / t_h;
458
458
}
@@ -463,7 +463,7 @@ FrenetPoint PathGenerator::getFrenetPoint(
463
463
const bool object_has_surpassed_limit_already = v > speed_limit;
464
464
if (terminal_velocity < speed_limit || object_has_surpassed_limit_already)
465
465
return v + a * (1.0 / lambda) +
466
- (a / (t_h * std::pow (lambda, 2 ))) * ( std::exp (-lambda * t_h) - 1 );
466
+ (a / (t_h * std::pow (lambda, 2 ))) * std::expm1 (-lambda * t_h);
467
467
468
468
// It is assumed the vehicle accelerates until final_speed is reached and
469
469
// then continues at constant speed for the rest of the time horizon
@@ -473,7 +473,7 @@ FrenetPoint PathGenerator::getFrenetPoint(
473
473
const double t_f = (-1.0 / lambda) * std::log (1 - ((speed_limit - v) * lambda) / a);
474
474
const double distance_covered =
475
475
// Distance covered while accelerating
476
- a * (1.0 / lambda) * t_f + a * (1.0 / std::pow (lambda, 2 )) * ( std::exp (-lambda * t_f) - 1 ) +
476
+ a * (1.0 / lambda) * t_f + a * (1.0 / std::pow (lambda, 2 )) * std::expm1 (-lambda * t_f) +
477
477
v * t_f +
478
478
// Distance covered at constant speed for the rest of the horizon time
479
479
speed_limit * (t_h - t_f);
0 commit comments