src/prediction.cpp
// Uses the separation of axis theorem to detect collisions
Uses the separation of axis theorem to detect collisions
Uses the separation of axis theorem to detect collisions
bool Prediction::checkCollision(double s0, double d0, double theta0, double s1, double d1, double theta1) {
  double safety_dist_lon = cfg_.carSafetyLength();
  double safety_dist_lat = cfg_.carSafetyWidth();
  Eigen::MatrixXd rec_wrapper(2, 4);
  rec_wrapper << safety_dist_lon, safety_dist_lon, -safety_dist_lon, -safety_dist_lon,
      -safety_dist_lat, safety_dist_lat, safety_dist_lat, -safety_dist_lat;
  Eigen::Matrix2d rot0, rot1;
  rot0 << cos(theta0), -sin(theta0), sin(theta0), cos(theta0);
  rot1 << cos(theta1), -sin(theta1), sin(theta1), cos(theta1);
  Eigen::MatrixXd rec0(2, 4);
  Eigen::MatrixXd rec1(2, 4);
  Eigen::Vector2d trans0, trans1;
  trans0 << s0, d0;
  trans1 << s1, d1;
  for (int i = 0; i < rec_wrapper.cols(); i++) {
    rec0.col(i) = rot0 * rec_wrapper.col(i) + trans0;
    rec1.col(i) = rot1 * rec_wrapper.col(i) + trans1;
  }
  Eigen::MatrixXd axis(2, 4);
  axis << cos(theta0), sin(theta0), cos(theta1), sin(theta1),
      sin(theta0), -cos(theta0), sin(theta1), -cos(theta1);
  for (int i = 0; i < axis.cols(); i++) {
    Eigen::Vector2d principal_axis = axis.col(i);
    double min0 = principal_axis.dot(rec0.col(0));
    double max0 = min0;
    for (int j = 0; j < rec0.cols(); j++) {
      double proj0 = principal_axis.dot(rec0.col(j));
      if (proj0 > max0) max0 = proj0;
      if (proj0 < min0) min0 = proj0;
    }
    double min1 = principal_axis.dot(rec1.col(0));
    double max1 = min1;
    for (int j = 0; j < rec1.cols(); j++) {
      double proj1 = principal_axis.dot(rec1.col(j));
      if (proj1 > max1) max1 = proj1;
      if (proj1 < min1) min1 = proj1;
    }
    if (!overlap({min0, max0}, {min1, max1})) return false;
  }
  return true;
}
bool Prediction :: checkCollision ( double s0 , double d0 , double theta0 , double s1 , double d1 , double theta1 ) { double safety_dist_lon = cfg_ . carSafetyLength ( ) ; double safety_dist_lat = cfg_ . carSafetyWidth ( ) ; Eigen :: MatrixXd rec_wrapper ( 2 , 4 ) ; rec_wrapper << safety_dist_lon , safety_dist_lon , - safety_dist_lon , - safety_dist_lon , - safety_dist_lat , safety_dist_lat , safety_dist_lat , - safety_dist_lat ; Eigen :: Matrix2d rot0 , rot1 ; rot0 << cos ( theta0 ) , - sin ( theta0 ) , sin ( theta0 ) , cos ( theta0 ) ; rot1 << cos ( theta1 ) , - sin ( theta1 ) , sin ( theta1 ) , cos ( theta1 ) ; Eigen :: MatrixXd rec0 ( 2 , 4 ) ; Eigen :: MatrixXd rec1 ( 2 , 4 ) ; Eigen :: Vector2d trans0 , trans1 ; trans0 << s0 , d0 ; trans1 << s1 , d1 ; for ( int i = 0 ; i < rec_wrapper . cols ( ) ; i ++ ) { rec0 . col ( i ) = rot0 * rec_wrapper . col ( i ) + trans0 ; rec1 . col ( i ) = rot1 * rec_wrapper . col ( i ) + trans1 ; } Eigen :: MatrixXd axis ( 2 , 4 ) ; axis << cos ( theta0 ) , sin ( theta0 ) , cos ( theta1 ) , sin ( theta1 ) , sin ( theta0 ) , - cos ( theta0 ) , sin ( theta1 ) , - cos ( theta1 ) ; for ( int i = 0 ; i < axis . cols ( ) ; i ++ ) { Eigen :: Vector2d principal_axis = axis . col ( i ) ; double min0 = principal_axis . dot ( rec0 . col ( 0 ) ) ; double max0 = min0 ; for ( int j = 0 ; j < rec0 . cols ( ) ; j ++ ) { double proj0 = principal_axis . dot ( rec0 . col ( j ) ) ; if ( proj0 > max0 ) max0 = proj0 ; if ( proj0 < min0 ) min0 = proj0 ; } double min1 = principal_axis . dot ( rec1 . col ( 0 ) ) ; double max1 = min1 ; for ( int j = 0 ; j < rec1 . cols ( ) ; j ++ ) { double proj1 = principal_axis . dot ( rec1 . col ( j ) ) ; if ( proj1 > max1 ) max1 = proj1 ; if ( proj1 < min1 ) min1 = proj1 ; } if ( ! overlap ( { min0 , max0 } , { min1 , max1 } ) ) return false ; } return true ; }
Uses the separation of axis theorem to detect collisions
Uses the separation of axis theorem to detect collisions
// set safety distance (to vehicle heading) // vehicle wrapper // rotate wrapper by heading // set principal axis list: normal + normal perpendicular // projection of rec0: get min, max // projection of rec1: get min, max // check overlap