src/prediction.cpp
// Check for an individual other car if a collision can occur
Check for an individual other car if a collision can occur
Check for an individual other car if a collision can occur
bool Prediction::checkIndividualOtherCar(XYPoints &pts, TrajectoryCandidate &tc) {
  assert(pts.xs.size() >= cfg_.planAhead());
  for (int i = 0; i < (int)(cfg_.traverseTime() / 0.02); i++) {
    double obj_x = pts.xs[i];
    double obj_y = pts.ys[i];
    double obj_x_next = pts.xs[i + 1];
    double obj_y_next = pts.ys[i + 1];
    double obj_heading = atan2(obj_y_next - obj_y, obj_x_next - obj_x);
    double ego_x = tc.jmt_traj.trajectory.pts.xs[i];
    double ego_y = tc.jmt_traj.trajectory.pts.ys[i];
    double ego_x_next = tc.jmt_traj.trajectory.pts.xs[i + 1];
    double ego_y_next = tc.jmt_traj.trajectory.pts.xs[i + 1];
    double ego_heading = atan2(ego_y_next - ego_y, ego_x_next - ego_x);
    if (checkCollision(obj_x, obj_y, obj_heading, ego_x, ego_y, ego_heading)) 
      return false;
  }
  return true;
}
bool Prediction :: checkIndividualOtherCar ( XYPoints & pts , TrajectoryCandidate & tc ) { assert ( pts . xs . size ( ) >= cfg_ . planAhead ( ) ) ; for ( int i = 0 ; i < ( int ) ( cfg_ . traverseTime ( ) / 0.02 ) ; i ++ ) { double obj_x = pts . xs [ i ] ; double obj_y = pts . ys [ i ] ; double obj_x_next = pts . xs [ i + 1 ] ; double obj_y_next = pts . ys [ i + 1 ] ; double obj_heading = atan2 ( obj_y_next - obj_y , obj_x_next - obj_x ) ; double ego_x = tc . jmt_traj . trajectory . pts . xs [ i ] ; double ego_y = tc . jmt_traj . trajectory . pts . ys [ i ] ; double ego_x_next = tc . jmt_traj . trajectory . pts . xs [ i + 1 ] ; double ego_y_next = tc . jmt_traj . trajectory . pts . xs [ i + 1 ] ; double ego_heading = atan2 ( ego_y_next - ego_y , ego_x_next - ego_x ) ; if ( checkCollision ( obj_x , obj_y , obj_heading , ego_x , ego_y , ego_heading ) ) return false ; } return true ; }
Check for an individual other car if a collision can occur
Check for an individual other car if a collision can occur
// Check for collision during the time span of the lane change