| | | 1 | | using System; |
| | | 2 | | using System.Collections.Generic; |
| | | 3 | | using UnityEngine; |
| | | 4 | | |
| | | 5 | | // The maximum speed assignment class assigns interceptors to the targets to maximize the intercept |
| | | 6 | | // speed by defining a cost of the assignment equal to the speed lost for the maneuver. |
| | | 7 | | public class MaxSpeedAssignment : IAssignment { |
| | | 8 | | // Assign a target to each interceptor that has not been assigned a target yet. |
| | | 9 | | public unsafe IEnumerable<IAssignment.AssignmentItem> Assign( |
| | 0 | 10 | | in IReadOnlyList<Interceptor> interceptors, in IReadOnlyList<Threat> threats) { |
| | 0 | 11 | | List<IAssignment.AssignmentItem> assignments = new List<IAssignment.AssignmentItem>(); |
| | | 12 | | |
| | 0 | 13 | | List<Interceptor> assignableInterceptors = IAssignment.GetAssignableInterceptors(interceptors); |
| | 0 | 14 | | if (assignableInterceptors.Count == 0) { |
| | 0 | 15 | | Debug.LogWarning("No assignable interceptors found."); |
| | 0 | 16 | | return assignments; |
| | | 17 | | } |
| | | 18 | | |
| | 0 | 19 | | List<Threat> activeThreats = IAssignment.GetActiveThreats(threats); |
| | 0 | 20 | | if (activeThreats.Count == 0) { |
| | 0 | 21 | | Debug.LogWarning("No active threats found."); |
| | 0 | 22 | | return assignments; |
| | | 23 | | } |
| | | 24 | | |
| | | 25 | | // Find all pairwise assignment costs. |
| | 0 | 26 | | float[] assignmentCosts = new float[assignableInterceptors.Count * activeThreats.Count]; |
| | 0 | 27 | | for (int interceptorIndex = 0; interceptorIndex < assignableInterceptors.Count; |
| | 0 | 28 | | ++interceptorIndex) { |
| | 0 | 29 | | Interceptor interceptor = assignableInterceptors[interceptorIndex]; |
| | | 30 | | |
| | | 31 | | // The speed decays exponentially with the travelled distance and with the bearing change. |
| | 0 | 32 | | float distanceTimeConstant = |
| | | 33 | | 2 * interceptor.staticAgentConfig.bodyConfig.mass / |
| | | 34 | | ((float)Constants.CalculateAirDensityAtAltitude(interceptor.GetPosition().y) * |
| | | 35 | | interceptor.staticAgentConfig.liftDragConfig.dragCoefficient * |
| | | 36 | | interceptor.staticAgentConfig.bodyConfig.crossSectionalArea); |
| | 0 | 37 | | float angleTimeConstant = interceptor.staticAgentConfig.liftDragConfig.liftDragRatio; |
| | | 38 | | // During the turn, the minimum radius dictates the minimum distance needed to make the turn. |
| | 0 | 39 | | float minTurningRadius = (float)(interceptor.GetVelocity().sqrMagnitude / |
| | | 40 | | interceptor.CalculateMaxNormalAcceleration()); |
| | | 41 | | |
| | 0 | 42 | | for (int threatIndex = 0; threatIndex < activeThreats.Count; ++threatIndex) { |
| | 0 | 43 | | Threat threat = activeThreats[threatIndex]; |
| | 0 | 44 | | Vector3 directionToThreat = threat.GetPosition() - interceptor.GetPosition(); |
| | 0 | 45 | | float distanceToThreat = directionToThreat.magnitude; |
| | 0 | 46 | | float angleToThreat = |
| | | 47 | | Vector3.Angle(interceptor.GetVelocity(), directionToThreat) * Mathf.Deg2Rad; |
| | | 48 | | |
| | | 49 | | // The fractional speed is the product of the fractional speed after traveling the distance |
| | | 50 | | // and of the fractional speed after turning. |
| | 0 | 51 | | float fractionalSpeed = Mathf.Exp( |
| | | 52 | | -((distanceToThreat + angleToThreat * minTurningRadius) / distanceTimeConstant + |
| | | 53 | | angleToThreat / angleTimeConstant)); |
| | 0 | 54 | | float cost = (float)interceptor.GetSpeed() / fractionalSpeed; |
| | 0 | 55 | | assignmentCosts[interceptorIndex * activeThreats.Count + threatIndex] = cost; |
| | 0 | 56 | | } |
| | 0 | 57 | | } |
| | | 58 | | |
| | | 59 | | // Solve the assignment problem. |
| | 0 | 60 | | int[] assignedInterceptorIndices = new int[assignableInterceptors.Count]; |
| | 0 | 61 | | int[] assignedThreatIndices = new int[assignableInterceptors.Count]; |
| | 0 | 62 | | int numAssignments = 0; |
| | 0 | 63 | | fixed(int* assignedInterceptorIndicesPtr = assignedInterceptorIndices) |
| | 0 | 64 | | fixed(int* assignedThreatIndicesPtr = assignedThreatIndices) { |
| | 0 | 65 | | numAssignments = Assignment.Assignment_EvenAssignment_Assign( |
| | | 66 | | assignableInterceptors.Count, activeThreats.Count, assignmentCosts, |
| | | 67 | | (IntPtr)assignedInterceptorIndicesPtr, (IntPtr)assignedThreatIndicesPtr); |
| | 0 | 68 | | } |
| | 0 | 69 | | for (int i = 0; i < numAssignments; ++i) { |
| | 0 | 70 | | int interceptorIndex = assignedInterceptorIndices[i]; |
| | 0 | 71 | | int threatIndex = assignedThreatIndices[i]; |
| | 0 | 72 | | assignments.Add(new IAssignment.AssignmentItem(assignableInterceptors[interceptorIndex], |
| | | 73 | | activeThreats[threatIndex])); |
| | 0 | 74 | | } |
| | 0 | 75 | | return assignments; |
| | 0 | 76 | | } |
| | | 77 | | } |