Implement simple collision detection

project_11_path_planning/src/main.cpp

@@ -195,7 +195,7 @@ int main() {
   int lane = 1;
 
   // Reference velocity (mph)
-  double ref_vel = 49.5; 
+  double ref_vel = 0.0; 
 
   h.onMessage([&map_waypoints_x,&map_waypoints_y,&map_waypoints_s,&map_waypoints_dx,&map_waypoints_dy,&ref_vel, &lane](uWS::WebSocket<uWS::SERVER> ws, char *data, size_t length,
                      uWS::OpCode opCode) {
@@ -236,6 +236,35 @@ int main() {
 
 			int prev_size = previous_path_x.size();
 
+			if (prev_size > 0)
+				car_s = end_path_s;
+
+			bool too_close = false;
+
+			for (size_t i = 0; i < sensor_fusion.size(); ++i) {
+
+				// Check if the `i_th` car is on our lane
+				double d = sensor_fusion[i][6];
+				if (d < (2 + 4 * lane + 2) && d >(2 + 4 * lane - 2)) {
+					double vx = sensor_fusion[i][3];
+					double vy = sensor_fusion[i][4];
+					double check_speed = sqrt(vx * vx + vy * vy);  // speed module
+					double check_car_s = sensor_fusion[i][5];
+
+					// use previous points to project s value outward
+					check_car_s += (double)prev_size * 0.02 * check_speed;
+
+					if (check_car_s > car_s && (check_car_s - car_s) < 30)
+						// add the logic here, for example either decrease speed or prepare for lane change
+						too_close = true;
+				}
+			}
+
+			if (too_close)
+				ref_vel -= 0.224; // around 5 m/s^2
+			else if (ref_vel < 49.5)
+				ref_vel += 0.224;
+
 			// List of widely spaced (x, y) waypoints. These will be later interpolated with a spline,
 			// filling it with more points which control speed.
 			vector<double> pts_x;