From aea24c6de4761caf56ba643fc149cbfa21a753c0 Mon Sep 17 00:00:00 2001
From: Brandon Danyluk <brandondanyluk366@gmail.com>
Date: Tue, 20 Apr 2021 15:31:38 -0600
Subject: [PATCH] Better attack angle calculation. Does not get exactly right,
 but works for all cases.

Need to implement a PID controller navigation type to get a correct attack angle
---
 .../functions/fnc_attackProfile_JDAM.sqf      | 39 ++++++++++---------
 .../functions/fnc_gps_attackOnFired.sqf       |  1 -
 2 files changed, 20 insertions(+), 20 deletions(-)

diff --git a/addons/missileguidance/functions/fnc_attackProfile_JDAM.sqf b/addons/missileguidance/functions/fnc_attackProfile_JDAM.sqf
index 8f5176d360..e49432ad5e 100644
--- a/addons/missileguidance/functions/fnc_attackProfile_JDAM.sqf
+++ b/addons/missileguidance/functions/fnc_attackProfile_JDAM.sqf
@@ -20,7 +20,7 @@
 params ["_seekerTargetPos", "_args", "_attackProfileStateParams", "_timestep"];
 _args params ["_firedEH", "", "_flightParams", "", "", "_targetData"];
 _firedEH params ["_shooter","","","","","","_projectile"];
-_attackProfileStateParams params ["_gpsData", "_initialProjectileHeight", "_terminal", "_lastAngleToTarget"];
+_attackProfileStateParams params ["_gpsData", "_initialProjectileHeight", "_terminal"];
 _gpsData params ["", "_impactAngle", "_attackDirection"];
 _targetData params ["_directionToTarget", "", "_distanceToTarget"];
 _flightParams params ["_pitchRate", "_yawRate"];
@@ -33,6 +33,10 @@ if (_attackDirection < 0) then {
 	_attackDirection = direction _projectile;
 };
 
+private _projectilePos = getPosASLVisual _projectile;
+private _projectilePitch = ((vectorDir _projectile) call CBA_fnc_vect2polar) select 2;
+private _targetDir = _projectilePos vectorFromTo _seekerTargetPos;
+
 private _targetPos = _seekerTargetPos;
 if !(_terminal) then {
 	_targetPos = [
@@ -40,32 +44,29 @@ if !(_terminal) then {
 		_targetPos#1,
 		(_seekerTargetPos select 2) + 500
 	];
+	
+	private _lineDir = [1, 180 + _attackDirection, _impactAngle] call CBA_fnc_polar2vect;
 
-	private _projectilePos = getPosASLVisual _projectile;
-	private _timeToGo = (_projectilePos distance _targetPos) / vectorMagnitude velocity _projectile;
+	private _v = _projectilePos vectorDiff _seekerTargetPos;
+	private _d = _v vectorDotProduct _lineDir;
+	private _closestPoint = _seekerTargetPos vectorAdd (_lineDir vectorMultiply _d);
 
-	private _currentPitchTowardTarget = ((_projectilePos vectorFromTo _seekerTargetPos) call CBA_fnc_vect2polar) select 2;
-	private _closingRate = if (_timestep != 0) then {
-		(_currentPitchTowardTarget - _lastAngleToTarget) / _timestep;
-	} else {	
-		0
-	};
-	_attackProfileStateParams set [3, _currentPitchTowardTarget];
+	private _timeToGo = (_projectilePos distance _closestPoint) / vectorMagnitude velocity _projectile;
 
-	private _pitchTime = _pitchRate * _timeToGo;
+	private _projectileAngleFromTarget = acos ((vectorDir _projectile) vectorCos _targetDir);
+	private _availablePitch = _pitchRate * _timeToGo;
 
-	private _projectileElevation = ((vectorDir _projectile) call CBA_fnc_vect2polar) select 2;
-	private _neededPitch = (_impactAngle + _projectileElevation + _closingRate);
-	private _pitchOverETA = _timeToGo - ((_impactAngle + _projectileElevation) / _pitchRate);
+	private _neededPitch = _impactAngle + _projectilePitch;
 
-	// TODO: look into PID controller and custom navigation type to ride the line? Same as SACLOS really
-
-	private _atMinRotationAngle = _pitchTime <= _neededPitch;
-	_attackProfileStateParams set [2, _atMinRotationAngle];
+	private _atMinRotationAngle = _availablePitch <= _neededPitch;
+	_attackProfileStateParams set [2, (_atMinRotationAngle || (_neededPitch <= _projectileAngleFromTarget))];
 
 	if (GVAR(debug_drawGuidanceInfo)) then {
-		_attackProfileName = format ["JDAM [Pitch Available - %1 Needed Pitch - %2 ETP - %3]", _pitchTime, _neededPitch, _pitchOverETA];
+		_attackProfileName = format ["JDAM [Pitch Available - %1 Needed Pitch - %2 TTP - %3]", _availablePitch, _neededPitch, (_availablePitch - _neededPitch) / _pitchRate];
+		drawIcon3D ["\a3\ui_f\data\IGUI\Cfg\Cursors\selectover_ca.paa", [1,0,0,1], ASLtoAGL _closestPoint, 0.75, 0.75, 0, "P", 1, 0.025, "TahomaB"];
 	};
+} else {
+	_attackProfileName = format ["JDAM [Pitch - %1  Impact Pitch - %2]", _projectilePitch, _impactAngle];
 };
 
 if (GVAR(debug_drawGuidanceInfo)) then {
diff --git a/addons/missileguidance/functions/fnc_gps_attackOnFired.sqf b/addons/missileguidance/functions/fnc_gps_attackOnFired.sqf
index fc1af427c6..fd633c324d 100644
--- a/addons/missileguidance/functions/fnc_gps_attackOnFired.sqf
+++ b/addons/missileguidance/functions/fnc_gps_attackOnFired.sqf
@@ -21,4 +21,3 @@ _firedEH params ["_shooter","","","","_ammo","","_projectile"];
 _attackProfileStateParams set [0, [] call FUNC(gps_getAttackData)];
 _attackProfileStateParams set [1, (getPosASL _projectile) select 2];
 _attackProfileStateParams set [2, false];
-_attackProfileStateParams set [3, 0];