Missiles lead collision/proportional navigation

Community Forums/Showcase/Missiles lead collision/proportional navigation

Flanker512

(Posted 2015) [#1]

Hi, I've been playing with lead collision course (called proportional navigation in missiles) and I recently came across a simple solution with trigonometry that I wanted to share.

Lead collision course is well known by sailors to avoid collision with other ships. The rule is quite simple, if your ship gets closer to another ship, and if the other ship stays at the exact same angle, you are on a collision course.

This is also used to guide modern missiles (in fact modern missiles are a lot more complex). It makes a missile far more effective than pure pursuit (simply following the target). Impact probality is improved on maneuvering targets, fuel consumption is optimised (increasing the missile range), and viewed from the target, the missile stays at the same angles, making it hard to spot.

I explain the trigonometry procedure I came across in an image :

Click here for full size image

Here is the code in two functions for 3d (for 2d just consider z=0) :

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Lead collision vector calculation (Flanker, 2015)
;;
;; The function LeadCollision calculates an interception vector from an origin entity (called missile) to a target entity, based on the absolute missile speed and target vector speed.
;; It is called "lead collision course", or "proportional navigation" for missiles.
;; The function returns True if a solution is found, or False if the target evades too fast.
;; The resulting vector is filled in the variables leadCol_vx#,leadCol_vy#,leadCol_vz# and should be applied to the origin entity to hit the target entity.
;; The impact point coordinates (optional) is filled in the variables leadCol_x#,leadCol_y#,leadCol_z#.

Global leadCol_vx#,leadCol_vy#,leadCol_vz#
Global leadCol_x#,leadCol_y#,leadCol_z#
Global lineSphere_x1#,lineSphere_y1#,lineSphere_z1#,lineSphere_x2#,lineSphere_y2#,lineSphere_z2#

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
Function LeadCollision(missile_x#,missile_y#,missile_z#,missile_speed#,target_x#,target_y#,target_z#,target_vx#,target_vy#,target_vz#)

	; first, substract the target velocity vector from the missile position to get the vector origin point
	vector_x# = missile_x - target_vx
	vector_y# = missile_y - target_vy
	vector_z# = missile_z - target_vz
	
	; now, find the intersection point beetween the direct line from target to missile and the missile's sphere (missile's sphere is a sphere position at vector origin with radius = missile speed)
	solution = LineToSphere_Intersection(target_x,target_y,target_z,missile_x,missile_y,missile_z,vector_x,vector_y,vector_z,missile_speed)
	
	If solution = 0 Then Return 0 ; if solution = 0, the target is escaping too fast for the missile
	
	; updating the new vector to target for the missile
	leadCol_vx = lineSphere_x1 - vector_x
	leadCol_vy = lineSphere_y1 - vector_y
	leadCol_vz = lineSphere_z1 - vector_z
	
	; OPTIONNAL : calculation of impact point
	; { 
	target_distance# = Distance3D(missile_x,missile_y,missile_z,target_x,target_y,target_z)
		
	vector_distance# = Distance3D(missile_x,missile_y,missile_z,lineSphere_x1,lineSphere_y1,lineSphere_z1)
	
	coeff# = target_distance / vector_distance
	
	leadCol_x = missile_x + leadCol_vx * coeff
	leadCol_y = missile_y + leadCol_vy * coeff
	leadCol_z = missile_z + leadCol_vz * coeff
	; }
	
	Return 1

End Function

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
Function LineToSphere_Intersection(lx1#,ly1#,lz1#,lx2#,ly2#,lz2#,cx#,cy#,cz#,cr#)

		px# = lx1
		py# = ly1
		pz# = lz1

        vx# = lx2 - px
		vy# = ly2 - py
		vz# = lz2 - pz

        a# = vx * vx + vy * vy + vz * vz
        b# = 2.0 * (px * vx + py * vy + pz * vz - vx * cx - vy * cy - vz * cz)
        c# = px * px - 2.0 * px * cx + cx * cx + py * py - 2.0 * py * cy + cy * cy + pz * pz - 2.0 * pz * cz + cz * cz - cr * cr

        d# = b * b - 4.0 * a * c

        If d < 0 Then Return 0
		
        t1# = ( -b - Sqr(d) ) / ( 2.0 * a )

		lineSphere_x1# = lx1 * (1.0 - t1) + t1 * lx2
		lineSphere_y1# = ly1 * (1.0 - t1) + t1 * ly2
		lineSphere_z1# = lz1 * (1.0 - t1) + t1 * lz2

		If d = 0 Then Return 1

        t2# = ( -b + Sqr(d) ) / ( 2.0 * a )
		lineSphere_x2# = lx1 * (1.0 - t2) + t2 * lx2
		lineSphere_y2# = ly1 * (1.0 - t2) + t2 * ly2
		lineSphere_z2# = lz1 * (1.0 - t2) + t2 * lz2
		
		Return 2

End Function

Just call LeadCollision with proper parameters and get your lead collision vector in leadCol_vx#,leadCol_vy#,leadCol_vz# variables. You can also find the impact point (approximative). It can be useful for a missile for example, to see if the impact point is not in the ground...

And finally, here is an example in use with Blitz3D, comparing proportional navigation with pure pursuit : Download archive
Both missiles are exactly the same physically (turn rate, cone angle, speed, acceleration...), just the guidance is different, and you'll see the difference of effectiveness. Sorry for the explosions, they are not optimised (simple bunch of sprites), so it can drop fps...

Hope it can help, I didn't find this kind of stuff in the code archive.

Flanker

(Posted 2015) [#2]

A little screenshot from the example downloadable above :

Pingus

(Posted 2015) [#3]

Waow ! If I had knew that when I was making action games ! I'll try to keep in mind this method which can be usefull.

Flanker

(Posted 2015) [#4]

Yes it can be useful, better than aproximating the impact point with lots of iterations (that was the method I used before...).

Floyd

(Posted 2015) [#5]

That calls a function, Distance3D, which is not included. No doubt something like this.

Function Distance3D#( x1#,y1#,z1#, x2#,y2#,z2# )
	Return Sqr( (x2-x1)*(x2-x1) + (y2-y1)*(y2-y1) + (z2-z1)*(z2-z1) )
End Function

Flanker

(Posted 2015) [#6]

Yes, exactly Floyd, forgot to include it.

Volturna

(Posted 2015) [#7]

Very nice and simple code! Was looking for something like that for homming missiles and cannon firing (shooting at future position instead of actual target coords).

Thank you for the post.

Blitzplotter

(Posted 2015) [#8]

whoa, very interesting post, thanks for sharing.

Flanker512

(Posted 2015) [#9]

I've been looking for this for a while too ! Finally with a pen and a paper I found this trigonometry method, wich in the end is an application of the Thales theorem.