< Summary

Class:IterativeLaunchPlanner
Assembly:bamlab.micromissiles
File(s):/github/workspace/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs
Covered lines:0
Uncovered lines:70
Coverable lines:70
Total lines:174
Line coverage:0% (0 of 70)
Covered branches:0
Total branches:0
Covered methods:0
Total methods:6
Method coverage:0% (0 of 6)

Metrics

MethodBranch coverage Crap Score Cyclomatic complexity NPath complexity Sequence coverage
IterativeLaunchPlanner(...)0%2100%
Plan()0%2100%
Plan(...)0%2100%
PlanFromZeroOrigin()0%42600%
PlanFromOrigin(...)0%42600%
IsInvalidLaunchGeometry(...)0%12300%

File(s)

/github/workspace/Assets/Scripts/Algorithms/Planning/IterativeLaunchPlanner.cs

#LineLine coverage
 1using UnityEngine;
 2
 3// The iterative launch planner class is a launch planner that performs an iterative process to
 4// determine the intercept point. The algorithm continuously performs the following 2-step iterative
 5// process:
 6//  1. Time-to-intercept estimation: The algorithm determines the time it takes the interceptor to
 7//  reach the target at the predicted intercept position, which is initialized to the target's
 8//  current position.
 9//  2. Intercept position estimation: The algorithm predicts the target position at the estimated
 10//  time-to-intercept.
 11public class IterativeLaunchPlanner : ILaunchPlanner {
 12  // Maximum number of iterations before declaring failure. In certain cases, the predictor and
 13  // planner do not converge, so we need to limit the number of iterations.
 14  private const int MaxNumIterations = 10;
 15
 16  // Convergence threshold in meters for the difference vector magnitude. Convergence is declared
 17  // when the intercept position has not changed by more than this threshold between iterations.
 18  private const float ConvergenceThreshold = 10f;
 19
 20  // Maximum intercept position threshold in meters to declare convergence. This threshold is used
 21  // as a final sanity check to ensure that the predicted target position and intercept position do
 22  // not differ by more than this threshold. This threshold should be set depending on the
 23  // granularity of the possible intercept positions.
 24  private const float InterceptPositionThreshold = 1000f;
 25
 26  public IterativeLaunchPlanner(ILaunchAnglePlanner launchAnglePlanner, IPredictor predictor)
 027      : base(launchAnglePlanner, predictor) {}
 28
 29  // Plan the launch.
 030  public override LaunchPlan Plan() {
 031    return PlanFromZeroOrigin();
 032  }
 33
 34  // Plan the launch from a specific interceptor origin.
 35  // This implementation accounts for the interceptor's starting position and the origin's
 36  // current location (including movement for naval assets).
 37  //   origin: Interceptor origin object
 38  // Returns: Launch plan with timing and angle information
 039  public override LaunchPlan Plan(InterceptorOriginObject origin) {
 40    // Get the current origin position (accounts for moving origins)
 041    Vector3 originPosition = origin.GetPosition();
 042    return PlanFromOrigin(originPosition);
 043  }
 44
 45  // Original implementation for zero origin (0,0,0).
 46  // Preserved for backward compatibility with existing tests.
 47  // Returns: Launch plan with timing and angle information
 048  private LaunchPlan PlanFromZeroOrigin() {
 049    PredictorState initialState = _predictor.Predict(time: 0);
 050    Vector3 targetPosition = initialState.Position;
 51
 052    LaunchAngleOutput launchAngleOutput = new LaunchAngleOutput();
 053    Vector3 interceptPosition = new Vector3();
 054    for (int i = 0; i < MaxNumIterations; ++i) {
 55      // Estimate the time-to-intercept from the current origin position
 056      launchAngleOutput = _launchAnglePlanner.Plan(targetPosition);
 057      float timeToIntercept = launchAngleOutput.TimeToPosition;
 58
 59      // Estimate the target position at intercept time
 060      PredictorState predictedState = _predictor.Predict(timeToIntercept);
 061      targetPosition = predictedState.Position;
 62
 63      // Check whether the intercept position has converged
 064      Vector3 newInterceptPosition = _launchAnglePlanner.GetInterceptPosition(targetPosition);
 65
 066      if ((interceptPosition - newInterceptPosition).magnitude < ConvergenceThreshold) {
 067        interceptPosition = newInterceptPosition;
 068        break;
 69      }
 070      interceptPosition = newInterceptPosition;
 71
 72      // Check that the target is moving towards the intercept position relative to the threat's
 73      // initial position. This prevents launching when the threat is moving away from the predicted
 74      // intercept.
 075      Vector3 targetToInterceptPosition = interceptPosition - initialState.Position;
 076      Vector3 targetToPredictedPosition = targetPosition - initialState.Position;
 077      if (Vector3.Dot(targetToInterceptPosition, targetToPredictedPosition) < 0) {
 078        return LaunchPlan.NoLaunch;
 79      }
 080    }
 81
 82    // Check for backwards/sideways launch scenarios using proper geometric analysis.
 083    if (IsInvalidLaunchGeometry(Vector3.zero, interceptPosition, initialState)) {
 084      return LaunchPlan.NoLaunch;
 85    }
 86
 87    // Final validation: ensure intercept and predicted positions are reasonably close
 088    if (Vector3.Distance(interceptPosition, targetPosition) < InterceptPositionThreshold) {
 089      return new LaunchPlan(launchAngleOutput.LaunchAngle, interceptPosition);
 90    }
 91
 092    return LaunchPlan.NoLaunch;
 093  }
 94
 95  // Origin-aware implementation for non-zero origins.
 96  // This implementation properly accounts for interceptor starting position.
 97  //   originPosition: The position from which the interceptor will be launched
 98  // Returns: Launch plan with timing and angle information
 099  private LaunchPlan PlanFromOrigin(Vector3 originPosition) {
 0100    PredictorState initialState = _predictor.Predict(time: 0);
 0101    Vector3 targetPosition = initialState.Position;
 102
 0103    LaunchAngleOutput launchAngleOutput = new LaunchAngleOutput();
 0104    Vector3 interceptPosition = new Vector3();
 105
 0106    for (int i = 0; i < MaxNumIterations; ++i) {
 107      // Estimate the time-to-intercept from the current origin position
 0108      launchAngleOutput = _launchAnglePlanner.Plan(targetPosition, originPosition);
 0109      float timeToIntercept = launchAngleOutput.TimeToPosition;
 110
 111      // Estimate the target position at intercept time
 0112      PredictorState predictedState = _predictor.Predict(timeToIntercept);
 0113      targetPosition = predictedState.Position;
 114
 115      // Check whether the intercept position has converged
 0116      Vector3 newInterceptPosition =
 117          _launchAnglePlanner.GetInterceptPosition(targetPosition, originPosition);
 0118      if ((interceptPosition - newInterceptPosition).magnitude < ConvergenceThreshold) {
 0119        interceptPosition = newInterceptPosition;
 0120        break;
 121      }
 0122      interceptPosition = newInterceptPosition;
 123
 124      // Check that the target is moving towards the intercept position relative to the threat's
 125      // initial position. This prevents launching when the threat is moving away from the predicted
 126      // intercept.
 0127      Vector3 targetToInterceptPosition = interceptPosition - initialState.Position;
 0128      Vector3 targetToPredictedPosition = targetPosition - initialState.Position;
 0129      if (Vector3.Dot(targetToInterceptPosition, targetToPredictedPosition) < 0) {
 0130        return LaunchPlan.NoLaunch;
 131      }
 0132    }
 133
 134    // Check for backwards/sideways launch scenarios using proper geometric analysis.
 0135    if (IsInvalidLaunchGeometry(originPosition, interceptPosition, initialState)) {
 0136      return LaunchPlan.NoLaunch;
 137    }
 138
 139    // Final validation: ensure intercept and predicted positions are reasonably close
 0140    if (Vector3.Distance(interceptPosition, targetPosition) < InterceptPositionThreshold) {
 0141      return new LaunchPlan(launchAngleOutput.LaunchAngle, interceptPosition);
 142    }
 143
 0144    return LaunchPlan.NoLaunch;
 0145  }
 146
 147  // Determines if a launch scenario is geometrically invalid (e.g., backwards or sideways).
 148  //   originPosition: Position from which the interceptor will be launched.
 149  //   interceptPosition: Calculated intercept position.
 150  //   threatState: Initial state of the threat (position and velocity).
 151  // Returns: True if the launch geometry is invalid and should be prevented.
 152  private bool IsInvalidLaunchGeometry(Vector3 originPosition, Vector3 interceptPosition,
 0153                                       PredictorState threatState) {
 0154    Vector3 originToThreat = threatState.Position - originPosition;
 0155    Vector3 threatVelocity = threatState.Velocity;
 156
 157    // A launch is invalid if the threat is moving away from the origin.
 158    // A dot product > 0 means the angle between the vector from the origin to the threat
 159    // and its velocity is < 90 degrees, indicating it's moving away.
 0160    if (Vector3.Dot(originToThreat, threatVelocity) > 0.0f) {
 0161      return true;
 162    }
 163
 164    // A launch is also invalid if the intercept point is "behind" the origin
 165    // relative to the threat's direction of approach. "Behind" means the angle
 166    // between the vector to the threat and the vector to the intercept point is > 90 degrees.
 0167    Vector3 originToIntercept = interceptPosition - originPosition;
 0168    if (Vector3.Dot(originToIntercept.normalized, originToThreat.normalized) < 0.0f) {
 0169      return true;
 170    }
 171
 0172    return false;
 0173  }
 174}