Format all files
This commit is contained in:
Titan Yuan
@@ -3,63 +3,57 @@ using System.Collections.Generic;
|
||||
using JetBrains.Annotations;
|
||||
using UnityEngine;
|
||||
|
||||
public class Hydra70 : Missile
|
||||
{
|
||||
public class Hydra70 : Missile {
|
||||
private Vector3 _acceleration;
|
||||
private bool _submunitionsLaunched = false;
|
||||
|
||||
private Vector3 _acceleration;
|
||||
private bool _submunitionsLaunched = false;
|
||||
protected override void Update() {
|
||||
base.Update();
|
||||
|
||||
protected override void Update() {
|
||||
// Check if it's time to launch submunitions
|
||||
if (!_submunitionsLaunched &&
|
||||
(GetFlightPhase() == FlightPhase.MIDCOURSE || GetFlightPhase() == FlightPhase.BOOST) &&
|
||||
SimManager.Instance.GetElapsedSimulationTime() >=
|
||||
_agentConfig.submunitions_config.launch_config.launch_time) {
|
||||
SpawnSubmunitions();
|
||||
_submunitionsLaunched = true;
|
||||
}
|
||||
}
|
||||
|
||||
base.Update();
|
||||
protected override void UpdateMidCourse(double deltaTime) {
|
||||
Vector3 accelerationInput = Vector3.zero;
|
||||
// Calculate and set the total acceleration
|
||||
Vector3 acceleration = CalculateAcceleration(accelerationInput);
|
||||
GetComponent<Rigidbody>().AddForce(acceleration, ForceMode.Acceleration);
|
||||
_acceleration = acceleration;
|
||||
}
|
||||
|
||||
// Check if it's time to launch submunitions
|
||||
if (!_submunitionsLaunched && (GetFlightPhase() == FlightPhase.MIDCOURSE || GetFlightPhase() == FlightPhase.BOOST) &&
|
||||
SimManager.Instance.GetElapsedSimulationTime() >= _agentConfig.submunitions_config.launch_config.launch_time)
|
||||
{
|
||||
SpawnSubmunitions();
|
||||
_submunitionsLaunched = true;
|
||||
protected override void DrawDebugVectors() {
|
||||
base.DrawDebugVectors();
|
||||
if (_acceleration != null) {
|
||||
Debug.DrawRay(transform.position, _acceleration * 1f, Color.green);
|
||||
}
|
||||
}
|
||||
|
||||
public void SpawnSubmunitions() {
|
||||
Debug.Log("Spawning submunitions");
|
||||
// print the callstack
|
||||
Debug.Log(new System.Diagnostics.StackTrace().ToString());
|
||||
List<Missile> submunitions = new List<Missile>();
|
||||
switch (_agentConfig.submunitions_config.agent_config.missile_type) {
|
||||
case MissileType.MICROMISSILE:
|
||||
for (int i = 0; i < _agentConfig.submunitions_config.num_submunitions; i++) {
|
||||
AgentConfig convertedConfig =
|
||||
AgentConfig.FromSubmunitionAgentConfig(_agentConfig.submunitions_config.agent_config);
|
||||
|
||||
convertedConfig.initial_state.position = transform.position;
|
||||
convertedConfig.initial_state.velocity = GetComponent<Rigidbody>().velocity;
|
||||
Missile submunition = SimManager.Instance.CreateMissile(convertedConfig);
|
||||
submunitions.Add(submunition);
|
||||
Debug.Log("Created submunition");
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
protected override void UpdateMidCourse(double deltaTime)
|
||||
{
|
||||
Vector3 accelerationInput = Vector3.zero;
|
||||
// Calculate and set the total acceleration
|
||||
Vector3 acceleration = CalculateAcceleration(accelerationInput);
|
||||
GetComponent<Rigidbody>().AddForce(acceleration, ForceMode.Acceleration);
|
||||
_acceleration = acceleration;
|
||||
}
|
||||
|
||||
protected override void DrawDebugVectors()
|
||||
{
|
||||
base.DrawDebugVectors();
|
||||
if (_acceleration != null)
|
||||
{
|
||||
Debug.DrawRay(transform.position, _acceleration * 1f, Color.green);
|
||||
}
|
||||
}
|
||||
|
||||
public void SpawnSubmunitions() {
|
||||
Debug.Log("Spawning submunitions");
|
||||
// print the callstack
|
||||
Debug.Log(new System.Diagnostics.StackTrace().ToString());
|
||||
List<Missile> submunitions = new List<Missile>();
|
||||
switch(_agentConfig.submunitions_config.agent_config.missile_type) {
|
||||
case MissileType.MICROMISSILE:
|
||||
for (int i = 0; i < _agentConfig.submunitions_config.num_submunitions; i++) {
|
||||
AgentConfig convertedConfig = AgentConfig.FromSubmunitionAgentConfig(_agentConfig.submunitions_config.agent_config);
|
||||
|
||||
convertedConfig.initial_state.position = transform.position;
|
||||
convertedConfig.initial_state.velocity = GetComponent<Rigidbody>().velocity;
|
||||
Missile submunition = SimManager.Instance.CreateMissile(convertedConfig);
|
||||
submunitions.Add(submunition);
|
||||
Debug.Log("Created submunition");
|
||||
}
|
||||
break;
|
||||
}
|
||||
SimManager.Instance.AssignMissilesToTargets(submunitions);
|
||||
|
||||
}
|
||||
|
||||
SimManager.Instance.AssignMissilesToTargets(submunitions);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -2,84 +2,77 @@ using System.Collections;
|
||||
using System.Collections.Generic;
|
||||
using UnityEngine;
|
||||
|
||||
public class Micromissile : Missile
|
||||
{
|
||||
[SerializeField] private float _navigationGain = 5f; // Typically 3-5
|
||||
public class Micromissile : Missile {
|
||||
[SerializeField]
|
||||
private float _navigationGain = 5f; // Typically 3-5
|
||||
|
||||
private SensorOutput _sensorOutput;
|
||||
private Vector3 _accelerationCommand;
|
||||
private double _elapsedTime = 0;
|
||||
protected override void UpdateMidCourse(double deltaTime)
|
||||
{
|
||||
_elapsedTime += deltaTime;
|
||||
Vector3 accelerationInput = Vector3.zero;
|
||||
if (HasAssignedTarget())
|
||||
{
|
||||
// Update the target model (assuming we have a target model)
|
||||
// TODO: Implement target model update logic
|
||||
private SensorOutput _sensorOutput;
|
||||
private Vector3 _accelerationCommand;
|
||||
private double _elapsedTime = 0;
|
||||
protected override void UpdateMidCourse(double deltaTime) {
|
||||
_elapsedTime += deltaTime;
|
||||
Vector3 accelerationInput = Vector3.zero;
|
||||
if (HasAssignedTarget()) {
|
||||
// Update the target model (assuming we have a target model)
|
||||
// TODO: Implement target model update logic
|
||||
|
||||
// Correct the state of the target model at the sensor frequency
|
||||
float sensorUpdatePeriod = 1f / _agentConfig.dynamic_config.sensor_config.frequency;
|
||||
if (_elapsedTime >= sensorUpdatePeriod)
|
||||
{
|
||||
// TODO: Implement guidance filter to estimate state from sensor output
|
||||
// For now, we'll use the target's actual state
|
||||
_sensorOutput = GetComponent<Sensor>().Sense(_target);
|
||||
_elapsedTime = 0;
|
||||
}
|
||||
// Correct the state of the target model at the sensor frequency
|
||||
float sensorUpdatePeriod = 1f / _agentConfig.dynamic_config.sensor_config.frequency;
|
||||
if (_elapsedTime >= sensorUpdatePeriod) {
|
||||
// TODO: Implement guidance filter to estimate state from sensor output
|
||||
// For now, we'll use the target's actual state
|
||||
_sensorOutput = GetComponent<Sensor>().Sense(_target);
|
||||
_elapsedTime = 0;
|
||||
}
|
||||
|
||||
// Check whether the target should be considered a miss
|
||||
SensorOutput sensorOutput = GetComponent<Sensor>().Sense(_target);
|
||||
if(sensorOutput.velocity.range > 1000f) {
|
||||
this.MarkAsMiss();
|
||||
}
|
||||
// Check whether the target should be considered a miss
|
||||
SensorOutput sensorOutput = GetComponent<Sensor>().Sense(_target);
|
||||
if (sensorOutput.velocity.range > 1000f) {
|
||||
this.MarkAsMiss();
|
||||
}
|
||||
|
||||
// Calculate the acceleration input
|
||||
accelerationInput = CalculateAccelerationCommand(_sensorOutput);
|
||||
}
|
||||
|
||||
// Calculate and set the total acceleration
|
||||
Vector3 acceleration = CalculateAcceleration(accelerationInput, compensateForGravity: true);
|
||||
GetComponent<Rigidbody>().AddForce(acceleration, ForceMode.Acceleration);
|
||||
|
||||
|
||||
}
|
||||
private Vector3 CalculateAccelerationCommand(SensorOutput sensorOutput)
|
||||
{
|
||||
// Implement Proportional Navigation guidance law
|
||||
Vector3 accelerationCommand = Vector3.zero;
|
||||
|
||||
// Extract relevant information from sensor output
|
||||
float los_rate_az = sensorOutput.velocity.azimuth;
|
||||
float los_rate_el = sensorOutput.velocity.elevation;
|
||||
float closing_velocity = -sensorOutput.velocity.range; // Negative because closing velocity is opposite to range rate
|
||||
|
||||
// Navigation gain (adjust as needed)
|
||||
float N = _navigationGain;
|
||||
|
||||
// Calculate acceleration commands in azimuth and elevation planes
|
||||
float acc_az = N * closing_velocity * los_rate_az;
|
||||
float acc_el = N * closing_velocity * los_rate_el;
|
||||
|
||||
// Convert acceleration commands to missile body frame
|
||||
accelerationCommand = transform.right * acc_az + transform.up * acc_el;
|
||||
|
||||
// Clamp the acceleration command to the maximum acceleration
|
||||
float maxAcceleration = CalculateMaxAcceleration();
|
||||
accelerationCommand = Vector3.ClampMagnitude(accelerationCommand, maxAcceleration);
|
||||
|
||||
// Update the stored acceleration command for debugging
|
||||
_accelerationCommand = accelerationCommand;
|
||||
return accelerationCommand;
|
||||
// Calculate the acceleration input
|
||||
accelerationInput = CalculateAccelerationCommand(_sensorOutput);
|
||||
}
|
||||
|
||||
protected override void DrawDebugVectors()
|
||||
{
|
||||
base.DrawDebugVectors();
|
||||
if (_accelerationCommand != null)
|
||||
{
|
||||
Debug.DrawRay(transform.position, _accelerationCommand * 1f, Color.green);
|
||||
}
|
||||
}
|
||||
// Calculate and set the total acceleration
|
||||
Vector3 acceleration = CalculateAcceleration(accelerationInput, compensateForGravity: true);
|
||||
GetComponent<Rigidbody>().AddForce(acceleration, ForceMode.Acceleration);
|
||||
}
|
||||
private Vector3 CalculateAccelerationCommand(SensorOutput sensorOutput) {
|
||||
// Implement Proportional Navigation guidance law
|
||||
Vector3 accelerationCommand = Vector3.zero;
|
||||
|
||||
// Extract relevant information from sensor output
|
||||
float los_rate_az = sensorOutput.velocity.azimuth;
|
||||
float los_rate_el = sensorOutput.velocity.elevation;
|
||||
float closing_velocity =
|
||||
-sensorOutput.velocity
|
||||
.range; // Negative because closing velocity is opposite to range rate
|
||||
|
||||
// Navigation gain (adjust as needed)
|
||||
float N = _navigationGain;
|
||||
|
||||
// Calculate acceleration commands in azimuth and elevation planes
|
||||
float acc_az = N * closing_velocity * los_rate_az;
|
||||
float acc_el = N * closing_velocity * los_rate_el;
|
||||
|
||||
// Convert acceleration commands to missile body frame
|
||||
accelerationCommand = transform.right * acc_az + transform.up * acc_el;
|
||||
|
||||
// Clamp the acceleration command to the maximum acceleration
|
||||
float maxAcceleration = CalculateMaxAcceleration();
|
||||
accelerationCommand = Vector3.ClampMagnitude(accelerationCommand, maxAcceleration);
|
||||
|
||||
// Update the stored acceleration command for debugging
|
||||
_accelerationCommand = accelerationCommand;
|
||||
return accelerationCommand;
|
||||
}
|
||||
|
||||
protected override void DrawDebugVectors() {
|
||||
base.DrawDebugVectors();
|
||||
if (_accelerationCommand != null) {
|
||||
Debug.DrawRay(transform.position, _accelerationCommand * 1f, Color.green);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user