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

    @param x world x.
    @param y world y.
    @param theta world theta.    
    @return position in frenet frame.
*/
transform position in world frame to frenet frame.
transform position in world frame to frenet frame .
std::vector<double> Map::get_frenet_frame_position(double x, double y, double theta)
{
	int next_waypoint_index = get_next_waypoint(x,y, theta);
	int prev_waypoint_index = (0 == next_waypoint_index ? center_line_x.size() - 1 : next_waypoint_index - 1);
	double tx = center_line_x[next_waypoint_index]-center_line_x[prev_waypoint_index];
	double ty = center_line_y[next_waypoint_index]-center_line_y[prev_waypoint_index];
    double norm_t = sqrt(tx*tx + ty*ty);
    tx /= norm_t;
    ty /= norm_t;
	double dx = x - center_line_x[prev_waypoint_index];
	double dy = y - center_line_y[prev_waypoint_index];
	double norm_proj = (dx*tx+dy*ty);
	double proj_x = norm_proj * tx;
	double proj_y = norm_proj * ty;
    double frenet_s = center_line_s[prev_waypoint_index] + norm_proj; 
	double frenet_d = distance(dx,dy,proj_x,proj_y);
    double perp_x = dx - proj_x;
    double perp_y = dy - proj_y;
    double nx = center_line_dx[prev_waypoint_index];
    double ny = center_line_dy[prev_waypoint_index];
    if (perp_x*nx + perp_y*ny < 0.0) {
        frenet_d *= -1;
    }
	return {frenet_s,frenet_d};
}
std :: vector < double > Map :: get_frenet_frame_position ( double x , double y , double theta ) { int next_waypoint_index = get_next_waypoint ( x , y , theta ) ; int prev_waypoint_index = ( 0 == next_waypoint_index ? center_line_x . size ( ) - 1 : next_waypoint_index - 1 ) ; double tx = center_line_x [ next_waypoint_index ] - center_line_x [ prev_waypoint_index ] ; double ty = center_line_y [ next_waypoint_index ] - center_line_y [ prev_waypoint_index ] ; double norm_t = sqrt ( tx * tx + ty * ty ) ; tx /= norm_t ; ty /= norm_t ; double dx = x - center_line_x [ prev_waypoint_index ] ; double dy = y - center_line_y [ prev_waypoint_index ] ; double norm_proj = ( dx * tx + dy * ty ) ; double proj_x = norm_proj * tx ; double proj_y = norm_proj * ty ; double frenet_s = center_line_s [ prev_waypoint_index ] + norm_proj ; double frenet_d = distance ( dx , dy , proj_x , proj_y ) ; double perp_x = dx - proj_x ; double perp_y = dy - proj_y ; double nx = center_line_dx [ prev_waypoint_index ] ; double ny = center_line_dy [ prev_waypoint_index ] ; if ( perp_x * nx + perp_y * ny < 0.0 ) { frenet_d *= -1 ; } return { frenet_s , frenet_d } ; }
transform position in world frame to frenet frame.
transform position in world frame to frenet frame .
// normalized tangential vector: // translation relative to previous waypoint: // find the projection of translation onto t // frenet s: // frenet d: // determine the sign of d: