Missile

//Missile by Peter Sharpe
//For KSP with the kOS mod

//YOU MUST MANUALLY TARGET YOUR TARGET BEFORE EXECUTING THIS PROGRAM.

//Editable Parameters
set explosionradius to 8.
set iterations to 4.
set initialguess to 15.

Clearscreen.

//Do Targeting
print "Enemy heat signature acquired. Launching...".

//Launch Sequence
stage.
lock throttle to 1.
lock steering to ship:srfprograde.
wait until alt:radar>100 or velocity:surface:mag>100.
print "Launch sequence completed. Now tracking target.".

//Staging
when stage:solidfuel<0.001 then {stage. wait 0.01. If ship:maxthrust>0 {preserve.}.}.

//Set up outputs
print "Time to interception:" at (0,10).
print "Distance to Target:" at (0,12).
print "Steering Error:" at (0,14).

//Take steering control, lock it to "steeringangle"
set steeringangle to target:direction.
sas off.
lock steering to steeringangle.

//Arm the warhead to trigger with radius of "explosionradius" + 1 frame to decouple.
when t<explosionradius/50+0.05 or target:distance<50 then {stage. set vectordraw:show to false.}
print "Warhead armed. Initiating homing algorithm.".

//Homing Algorithm, infinite loop
set t to 10.
until 0
{
	set rpos to (0-1)*(target:Position).
	set rvel to (ship:velocity:surface-target:velocity:orbit).
	if target:loaded {set rvel to ship:velocity:surface-target:velocity:surface.}.
	if altitude>35000 or target:altitude>35000 {set rvel to ship:velocity:orbit-target:velocity:orbit.}.
	set amag to ship:maxthrust/(ship:mass*9.81).

	//Solve for t, where 0=at^4+bt^2+ct+d. First get coefficients
	set a to 0-((amag)^2)/4.
	set b to (rvel:sqrmagnitude).
	set c to 2*(rvel*rpos).
	set d to (rpos:sqrmagnitude).

	//Do a few Newton-Raphson iterations:
	set timeguesses to list().
	set timeguesses:add to initialguess.
	set position to 0.
	until position>=iterations
	{
	set timeguesses:add to timeguesses[position]-(a*timeguesses[position]^4+b*timeguesses[position]^2+c*timeguesses[position]+d)/(4*a*timeguesses[position]^3+2*b*timeguesses[position]+c).
	set position to position+1.
	}.
	set initialguess to abs(timeguesses[iterations]).

	//Then calculate your desired direction
	set t to abs(timeguesses[iterations])/1.15.
	if altitude>35000 {set t to abs(timeguesses[iterations]).}.
	print t at (22,10).
	print target:distance at (20,12).
	set steeringvector to (v((0-2)*(rpos:x+(rvel:x)*t)/(t^2),(0-2)*(rpos:y+(rvel:y)*t)/(t^2),(0-2)*(rpos:z+(rvel:z)*t)/(t^2))+6*(up:vector)).
	set steeringangle to steeringvector:direction.
	print vectorangle(steeringvector,ship:facing:vector) at (16,14).
	set Vectordraw TO VECDRAWARGS(v(0,0,0), steeringvector:normalized, rgb(1,0.5,0.5),"", 12.0, true ).
	wait 0.01.
}.