src/map.cpp
// Transform from Cartesian x,y coordinates to Frenet s,d coordinates
Transform from Cartesian x,y coordinates to Frenet s,d coordinates
Transform from Cartesian x y coordinates to Frenet s d coordinates
vector<double> Map::getFrenet(double x, double y, double theta) {
  int next_wp = NextWaypoint(x, y, theta);
  int prev_wp;
  prev_wp = next_wp - 1;
  if (next_wp == 0) {
    prev_wp = map_waypoints_x_.size() - 1;
  }
  double n_x =
      high_res_map_waypoints_x_[next_wp] - high_res_map_waypoints_x_[prev_wp];
  double n_y =
      high_res_map_waypoints_y_[next_wp] - high_res_map_waypoints_y_[prev_wp];
  double x_x = x - high_res_map_waypoints_x_[prev_wp];
  double x_y = y - high_res_map_waypoints_y_[prev_wp];
  double proj_norm = (x_x * n_x + x_y * n_y) / (n_x * n_x + n_y * n_y);
  double proj_x = proj_norm * n_x;
  double proj_y = proj_norm * n_y;
  double frenet_d = distance(x_x, x_y, proj_x, proj_y);
  double center_x = cfg_.centerX() - high_res_map_waypoints_x_[prev_wp];
  double center_y = cfg_.centerY() - high_res_map_waypoints_y_[prev_wp];
  double centerToPos = distance(center_x, center_y, x_x, x_y);
  double centerToRef = distance(center_x, center_y, proj_x, proj_y);
  if (centerToPos <= centerToRef) {
    frenet_d *= -1;
  }
  double frenet_s = 0;
  for (int i = 0; i < prev_wp; i++) {
    frenet_s += distance(
        high_res_map_waypoints_x_[i], high_res_map_waypoints_y_[i],
        high_res_map_waypoints_x_[i + 1], high_res_map_waypoints_y_[i + 1]);
  }
  frenet_s += distance(0, 0, proj_x, proj_y);
  assert(frenet_d >= 0);
  return {frenet_s, frenet_d};
}
vector < double > Map :: getFrenet ( double x , double y , double theta ) { int next_wp = NextWaypoint ( x , y , theta ) ; int prev_wp ; prev_wp = next_wp - 1 ; if ( next_wp == 0 ) { prev_wp = map_waypoints_x_ . size ( ) - 1 ; } double n_x = high_res_map_waypoints_x_ [ next_wp ] - high_res_map_waypoints_x_ [ prev_wp ] ; double n_y = high_res_map_waypoints_y_ [ next_wp ] - high_res_map_waypoints_y_ [ prev_wp ] ; double x_x = x - high_res_map_waypoints_x_ [ prev_wp ] ; double x_y = y - high_res_map_waypoints_y_ [ prev_wp ] ; double proj_norm = ( x_x * n_x + x_y * n_y ) / ( n_x * n_x + n_y * n_y ) ; double proj_x = proj_norm * n_x ; double proj_y = proj_norm * n_y ; double frenet_d = distance ( x_x , x_y , proj_x , proj_y ) ; double center_x = cfg_ . centerX ( ) - high_res_map_waypoints_x_ [ prev_wp ] ; double center_y = cfg_ . centerY ( ) - high_res_map_waypoints_y_ [ prev_wp ] ; double centerToPos = distance ( center_x , center_y , x_x , x_y ) ; double centerToRef = distance ( center_x , center_y , proj_x , proj_y ) ; if ( centerToPos <= centerToRef ) { frenet_d *= -1 ; } double frenet_s = 0 ; for ( int i = 0 ; i < prev_wp ; i ++ ) { frenet_s += distance ( high_res_map_waypoints_x_ [ i ] , high_res_map_waypoints_y_ [ i ] , high_res_map_waypoints_x_ [ i + 1 ] , high_res_map_waypoints_y_ [ i + 1 ] ) ; } frenet_s += distance ( 0 , 0 , proj_x , proj_y ) ; assert ( frenet_d >= 0 ) ; return { frenet_s , frenet_d } ; }
Transform from Cartesian x,y coordinates to Frenet s,d coordinates
Transform from Cartesian x y coordinates to Frenet s d coordinates
// find the projection of x onto n // see if d value is positive or negative by comparing it to a center point // calculate s value