habrador / self-driving-vehicle Goto Github PK

private static List<ReedSheppPathfindingCar> AddWaypoints(
	PathWords word,
	PathSegmentLengths pathSegmentLengths,
	ReedSheppPathfindingCar carStart,
	ReedSheppPathfindingCar carEnd,
	float wpDistance,
	float turningRadius,
	bool generateOneWp)
{
	// Find the car settings we need to drive through the path
	List<SegmentSettings> pathSettings = PathSettings.GetSettings(word, pathSegmentLengths);

	// Each segment starts from the second subsegment to prevent waypoints duplication. So, we need to add first waypoint manually
	List<ReedSheppPathfindingCar> waypoints = new List<ReedSheppPathfindingCar>();
	waypoints.Add(new ReedSheppPathfindingCar(carStart.Position, carStart.HeadingInRad, pathSettings[0].Gear, pathSettings[0].Steering));

	// Track last waypoint
	ReedSheppPathfindingCar lastWaypoint = waypoints[^1];

	// Loop through all 3-5 path segments
	foreach (SegmentSettings segmentSettings in pathSettings)
	{
		int subSegments = (int)Math.Ceiling(segmentSettings.Length * turningRadius / wpDistance);
		float interpolationPerSubsegment = wpDistance / (segmentSettings.Length * turningRadius);

		Vector3 segmentStartPosition = lastWaypoint.Position;
		float segmentStartHeading = lastWaypoint.HeadingInRad;

		if (segmentSettings.Steering == ReedSheppPathfindingCar.Steering.Straight)
		{
			float direction = segmentSettings.Gear == ReedSheppPathfindingCar.Gear.Back ? -1f : 1f;
			Vector3 delta = direction * segmentSettings.Length * turningRadius
				* new Vector3(Mathf.Sin(segmentStartHeading), 0f, Mathf.Cos(segmentStartHeading));
			Vector3 segmentEndPosition = segmentStartPosition + delta;

			for (int i = 1; i <= subSegments; i++)
			{
				float interpolation = Mathf.Clamp01(i * interpolationPerSubsegment);
				Vector3 position = Vector3.Lerp(segmentStartPosition, segmentEndPosition, interpolation);
				waypoints.Add(new ReedSheppPathfindingCar(position, segmentStartHeading, segmentSettings.Gear, segmentSettings.Steering));

				if (generateOneWp)
				{
					return waypoints;
				}
			}
		}
		else
		{
			float reverseIfLeft = segmentSettings.Steering == ReedSheppPathfindingCar.Steering.Left ? -1f : 1f;
			float reverseIfBack = segmentSettings.Gear == ReedSheppPathfindingCar.Gear.Back ? -1f : 1f;

			float steeringClockwise = reverseIfLeft * reverseIfBack;
			float steeringAngle = segmentSettings.Length * steeringClockwise;

			float displacementAngle = segmentStartHeading + Mathf.PI / 2f * reverseIfLeft;
			Vector3 displacement = new Vector3(Mathf.Sin(displacementAngle), 0f, Mathf.Cos(displacementAngle));

			for (int i = 1; i <= subSegments; i++)
			{
				float interpolation = Mathf.Clamp01(i * interpolationPerSubsegment);
				float currentSteering = steeringAngle * interpolation;

				float circleAngle = segmentStartHeading - Mathf.PI / 2f * reverseIfLeft + currentSteering;
				Vector3 circle = new Vector3(Mathf.Sin(circleAngle), 0f, Mathf.Cos(circleAngle));
				Vector3 delta = (circle + displacement) * turningRadius;

				Vector3 position = segmentStartPosition + delta;
				float heading = segmentStartHeading + currentSteering;
				waypoints.Add(new ReedSheppPathfindingCar(position, heading, segmentSettings.Gear, segmentSettings.Steering));

				if (generateOneWp)
				{
					return waypoints;
				}
			}
		}
		lastWaypoint = waypoints[^1];
	}

	// The accuracy of the last waypoint is about 1.E-5 for the position and 1.E-7 for the angle.
	// So, there no need for a correction, but better be safe then sorry.
	waypoints[^1] = new ReedSheppPathfindingCar(carEnd.Position, carEnd.HeadingInRad, lastWaypoint.CurrentGear, lastWaypoint.CurrentSteering);
	return waypoints;
}