src/map/map.cpp
/**
    transform position in frenet frame to world frame.

    @param s frenet s.
    @param d frenet d.
    @return position in world frame.
*/
transform position in frenet frame to world frame.
transform position in frenet frame to world frame .
std::vector<double> Map::get_world_frame_position(double s, double d)
{
    s = fmod(s, ROUND_TRIP_S);
    std::vector<double>::iterator upper = std::upper_bound(
        center_line_s.begin(), center_line_s.end(), 
        s
    );
    int N = center_line_s.size();
    int waypoint_end_id = (upper - center_line_s.begin()) % N;
    int waypoint_start_id = (waypoint_end_id + N - 1) % N;
	double heading = atan2(
        (center_line_y[waypoint_end_id]-center_line_y[waypoint_start_id]),
        (center_line_x[waypoint_end_id]-center_line_x[waypoint_start_id])
    );
	double seg_s = (s-center_line_s[waypoint_start_id]);
	double seg_x = center_line_x[waypoint_start_id]+seg_s*cos(heading);
	double seg_y = center_line_y[waypoint_start_id]+seg_s*sin(heading);
	double world_x = seg_x + d * center_line_dx[waypoint_start_id];
	double world_y = seg_y + d * center_line_dy[waypoint_start_id];
	return {world_x, world_y};
}
std :: vector < double > Map :: get_world_frame_position ( double s , double d ) { s = fmod ( s , ROUND_TRIP_S ) ; std :: vector < double > :: iterator upper = std :: upper_bound ( center_line_s . begin ( ) , center_line_s . end ( ) , s ) ; int N = center_line_s . size ( ) ; int waypoint_end_id = ( upper - center_line_s . begin ( ) ) % N ; int waypoint_start_id = ( waypoint_end_id + N - 1 ) % N ; double heading = atan2 ( ( center_line_y [ waypoint_end_id ] - center_line_y [ waypoint_start_id ] ) , ( center_line_x [ waypoint_end_id ] - center_line_x [ waypoint_start_id ] ) ) ; double seg_s = ( s - center_line_s [ waypoint_start_id ] ) ; double seg_x = center_line_x [ waypoint_start_id ] + seg_s * cos ( heading ) ; double seg_y = center_line_y [ waypoint_start_id ] + seg_s * sin ( heading ) ; double world_x = seg_x + d * center_line_dx [ waypoint_start_id ] ; double world_y = seg_y + d * center_line_dy [ waypoint_start_id ] ; return { world_x , world_y } ; }
transform position in frenet frame to world frame.
transform position in frenet frame to world frame .
// prevent overflow: // identify segment: