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
std::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 = map_waypoints_x_[next_wp] - map_waypoints_x_[prev_wp];
	double n_y = map_waypoints_y_[next_wp] - map_waypoints_y_[prev_wp];
	double x_x = x - map_waypoints_x_[prev_wp];
	double x_y = y - 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 = Map::distance(x_x, x_y, proj_x, proj_y);
	double center_x = 1000 - map_waypoints_x_[prev_wp];
	double center_y = 2000 - map_waypoints_y_[prev_wp];
	double centerToPos = Map::distance(center_x, center_y, x_x, x_y);
	double centerToRef = Map::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 += Map::distance(map_waypoints_x_[i], map_waypoints_y_[i], map_waypoints_x_[i + 1], map_waypoints_y_[i + 1]);
	}
	frenet_s += Map::distance(0, 0, proj_x, proj_y);
	return
	{
		frenet_s, frenet_d
	};
}
std :: 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 = map_waypoints_x_ [ next_wp ] - map_waypoints_x_ [ prev_wp ] ; double n_y = map_waypoints_y_ [ next_wp ] - map_waypoints_y_ [ prev_wp ] ; double x_x = x - map_waypoints_x_ [ prev_wp ] ; double x_y = y - 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 = Map :: distance ( x_x , x_y , proj_x , proj_y ) ; double center_x = 1000 - map_waypoints_x_ [ prev_wp ] ; double center_y = 2000 - map_waypoints_y_ [ prev_wp ] ; double centerToPos = Map :: distance ( center_x , center_y , x_x , x_y ) ; double centerToRef = Map :: 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 += Map :: distance ( map_waypoints_x_ [ i ] , map_waypoints_y_ [ i ] , map_waypoints_x_ [ i + 1 ] , map_waypoints_y_ [ i + 1 ] ) ; } frenet_s += Map :: distance ( 0 , 0 , proj_x , proj_y ) ; 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