| | | 1 | | using System; |
| | | 2 | | using System.Collections; |
| | | 3 | | using System.Collections.Generic; |
| | | 4 | | using System.IO; |
| | | 5 | | using UnityEngine; |
| | | 6 | | |
| | | 7 | | // The launch angle interpolator class determines the optimal launch angle and the time-to-target |
| | | 8 | | // given the horizontal distance and altitude of the target. |
| | | 9 | | public abstract class ILaunchAngleInterpolator : ILaunchAnglePlanner { |
| | | 10 | | // Launch angle data interpolator. |
| | | 11 | | protected IInterpolator2D _interpolator; |
| | | 12 | | |
| | | 13 | | public ILaunchAngleInterpolator() : base() {} |
| | | 14 | | |
| | | 15 | | // Initialize the interpolator. |
| | | 16 | | protected abstract void InitInterpolator(); |
| | | 17 | | |
| | | 18 | | // Calculate the optimal launch angle in degrees and the time-to-target in seconds. |
| | | 19 | | public LaunchAngleOutput Plan(in LaunchAngleInput input) { |
| | | 20 | | if (_interpolator == null) { |
| | | 21 | | InitInterpolator(); |
| | | 22 | | } |
| | | 23 | | |
| | | 24 | | Interpolator2DDataPoint interpolatedDataPoint = |
| | | 25 | | _interpolator.Interpolate(input.Distance, input.Altitude); |
| | | 26 | | |
| | | 27 | | if (interpolatedDataPoint == null || interpolatedDataPoint.Data == null || |
| | | 28 | | interpolatedDataPoint.Data.Count < 2) { |
| | | 29 | | throw new InvalidOperationException("Interpolator returned invalid data."); |
| | | 30 | | } |
| | | 31 | | |
| | | 32 | | return new LaunchAngleOutput(launchAngle: interpolatedDataPoint.Data[0], |
| | | 33 | | timeToPosition: interpolatedDataPoint.Data[1]); |
| | | 34 | | } |
| | | 35 | | |
| | | 36 | | // Get the intercept position. |
| | | 37 | | public Vector3 GetInterceptPosition(Vector3 position) { |
| | | 38 | | Vector2 direction = ILaunchAnglePlanner.ConvertToDirection(position); |
| | | 39 | | Interpolator2DDataPoint interpolatedDataPoint = |
| | | 40 | | _interpolator.Interpolate(direction[0], direction[1]); |
| | | 41 | | Vector3 cylindricalPosition = Coordinates3.ConvertCartesianToCylindrical(position); |
| | | 42 | | return Coordinates3.ConvertCylindricalToCartesian(r: interpolatedDataPoint.Coordinates[0], |
| | | 43 | | azimuth: cylindricalPosition.y, |
| | | 44 | | height: interpolatedDataPoint.Coordinates[1]); |
| | | 45 | | } |
| | | 46 | | } |
| | | 47 | | |
| | | 48 | | // The launch angle CSV interpolator class loads launch angle data from a CSV file and provides |
| | | 49 | | // interpolated values for arbitrary target positions. |
| | | 50 | | public class LaunchAngleCsvInterpolator : ILaunchAngleInterpolator { |
| | | 51 | | // Path to the CSV file. |
| | | 52 | | // The first two columns of the CSV file specify the coordinates of each data point. |
| | | 53 | | // The third column denotes the launch angle in degrees, and the fourth column denotes the time to |
| | | 54 | | // reach the target position. |
| | | 55 | | private readonly string _relativePath; |
| | | 56 | | |
| | | 57 | | // Delegate for loading the CSV file. |
| | | 58 | | public delegate string ConfigLoaderDelegate(string path); |
| | | 59 | | private readonly ConfigLoaderDelegate _configLoader; |
| | | 60 | | |
| | | 61 | | public LaunchAngleCsvInterpolator(string path = null, ConfigLoaderDelegate configLoader = null) |
| | | 62 | | : base() { |
| | | 63 | | _relativePath = path ?? Path.Combine("Planning", "hydra70_launch_angle.csv"); |
| | | 64 | | _configLoader = configLoader ?? ConfigLoader.LoadFromStreamingAssets; |
| | | 65 | | } |
| | | 66 | | |
| | | 67 | | // Initialize the interpolator. |
| | | 68 | | protected override void InitInterpolator() { |
| | | 69 | | string fileContent = _configLoader(_relativePath); |
| | | 70 | | if (string.IsNullOrEmpty(fileContent)) { |
| | | 71 | | Debug.LogError($"Failed to load CSV file from {_relativePath}."); |
| | | 72 | | throw new InvalidOperationException("Interpolator could not be initialized."); |
| | | 73 | | } |
| | | 74 | | |
| | | 75 | | string[] csvLines = fileContent.Split('\n'); |
| | | 76 | | if (csvLines.Length < 1) { |
| | | 77 | | throw new InvalidOperationException("No data points available for interpolation."); |
| | | 78 | | } |
| | | 79 | | |
| | | 80 | | try { |
| | | 81 | | _interpolator = new NearestNeighborInterpolator2D(csvLines); |
| | | 82 | | } catch (Exception e) { |
| | | 83 | | throw new InvalidOperationException("Failed to initialize interpolator: " + e.Message); |
| | | 84 | | } |
| | | 85 | | } |
| | | 86 | | } |
| | | 87 | | |
| | | 88 | | // The launch angle data interpolator class interpolates the values from a static list of values. |
| | | 89 | | public abstract class LaunchAngleDataInterpolator : ILaunchAngleInterpolator { |
| | 0 | 90 | | public LaunchAngleDataInterpolator() : base() {} |
| | | 91 | | |
| | | 92 | | // Initialize the interpolator. |
| | 0 | 93 | | protected override void InitInterpolator() { |
| | 0 | 94 | | List<Interpolator2DDataPoint> interpolatorDataPoints = new List<Interpolator2DDataPoint>(); |
| | 0 | 95 | | foreach (var dataPoint in GenerateData()) { |
| | 0 | 96 | | Interpolator2DDataPoint interpolatorDataPoint = new Interpolator2DDataPoint( |
| | | 97 | | new Vector2(dataPoint.Input.Distance, dataPoint.Input.Altitude), |
| | | 98 | | new List<float> { dataPoint.Output.LaunchAngle, dataPoint.Output.TimeToPosition }); |
| | 0 | 99 | | interpolatorDataPoints.Add(interpolatorDataPoint); |
| | 0 | 100 | | } |
| | 0 | 101 | | _interpolator = new NearestNeighborInterpolator2D(interpolatorDataPoints); |
| | 0 | 102 | | } |
| | | 103 | | |
| | | 104 | | // Generate the list of launch angle data points to interpolate. |
| | | 105 | | protected abstract List<LaunchAngleDataPoint> GenerateData(); |
| | | 106 | | } |