### Calculating a lead on a target

Recently, Quadgnim, on the Unity 3d forums, asked me if I could help him figure out how the code for leading a target would work.  I thought about it for a minute, and then thought about it for a few more minutes.  And now after several hours, I’ve decided it was one of the more fun derivations I’ve done in a while.  So I’m going to share.

First, I’ll lay out the problem.  Consider a sniper, “A” and a target, “B”.  Sniper A is stationary and trying to track a target who is moving, for simplicity at some fixed speed in some arbitrary 3-dimensional direction.  Sniper A wants to fire on B, but if he fires where he sees B, then by the time the bullet arrives, B may have moved out of the way.  So the Sniper must aim at where B will be when the bullet get there.

Here’s a picture of the situation.

A fires on B with the solid red line. But B has moved to the dotted position B prime. A should have fired with the dotted red line.

This would be very simple if B were moving directly toward A or directly away from A.  Then we could either subtract or add that speed to our projectile speed and solve.  But we must concern ourselves with the perpendicular speed as well.  So where do we start?

In order to solve this, we must find a missing variable.  We know all the variables except time, that is the time at which our projectile will hit the target if we aimed properly.  Technically we don’t know where to aim, but if we knew the time, then because B is moving at a steady velocity we would just aim at B’s position plus the distance he travelled over that time, that looks like this

$P_b + V_b \Delta t$, where $P_b$ is B’s position and $V_b$ is B’s velocity.

So all we need to do is find the $\Delta t$ and plug it into that equation and that’s where we know B will be and thus where Sniper A should aim.

But how do we actually solve this?  Well, let’s start with the naive approach: Lets just fire directly at B and solve for that $\Delta t$.  That’s pretty straight forward, we just need to find the distance from Sniper A to Target B and then divide by the speed of the projectile.  That looks like this:

$\Delta t_{naive} = \frac{||P_b - P_a||}{s}$ where $P_a$ and $P_b$ are A and B’s positions and $s$ is the speed of the projectile.

But the problem here is that we’ve missed the target by $V_b \Delta t_{naive}$.   Now we could take the approach that we find an initial $\Delta t$ this way, call it $\Delta t_1$ and then use that to compute a new $P_b$ and find a $\Delta t_2$.  That would look like this:

$\Delta t_2 = \frac{||P_b + V_b \Delta t_1 - P_a||}{s}$

We can continue to iterate this recurrence relation this way, finding $\Delta t_3, \Delta t_4 \ldots$ and so on.  This is a pretty good way to solve a non-linear problem like this, if we don’t know of a closed form solution.  We just keep iterating until, if we’re lucky, we get a converged value, a $\Delta t^*$.

First find the time it takes to reach B0, then compute how far B has moved to B1. Re run the algorithm using B1 to find B2 and so on.

However, this may not converge.  I’ll return to this because it’ll be there in any solution we find, but intuitively we can guess that it’s a case where our Target is moving as fast or faster than our Sniper’s projectile.  Of course, this wouldn’t happen when we’re talking about humans and bullets, but we may not always be talking about that.  Luckily with a recurrence relation like this, it’s easy to check if we’re converging, we simply watch the change in value between each $\Delta t_k$ and $\Delta t_{k+1}$ and make sure the number is getting smaller.  If it grows or stays the same, we can’t solve the problem.

public float projectileSpeed;
public float lastError = Mathf.Infinity;

Vector3 V = targetVelocity;
Vector3 D = previousPrediction - sniper.position;
float dt = D.magnitude / projectileSpeed;
Vector3 pos = target.position + V * dt;
float thisError = (pos - previousPrediction).magnitude;
if (thisError >= lastError) {
Debug.LogError ("No solution exists");
lastError = -1; // -1 is signaling its wrong, never should be negative
} else {
lastError = thisError; // outside code checks when this is close enough to 0.
}
return pos;
}

Here’s an example player showing this code working.

Okay, so we have a solution but it’s not really efficient.  If we could find a closed for solution, we could just plug the numbers in and get the answer back.  Maybe that’s possible.  Well let’s start with that recurrence relation we were using, but just get rid of the steps.  There’s only one $\Delta t$ that we want, so let’s plug that in to both sides of the equation.

$\Delta t = \frac{||P_b + V_b \Delta t - P_a||}{s}$

So, now we can’t easily find a solution for $\Delta t$ because its on both sides of the equation, so we have to do the hard work of seeing whether or not we can isolate it.  I’m out of practice; there’s probably a shortcut I’m missing.  In the end I’ll show the work in case there’s a mistake I’ve made along the way.

So first, I’ll move the $s$ out of the way.

$s\Delta t = ||P_b + V_b \Delta t - P_a||$

Square both sides because it’s a vector magnitude over there and I want to get rid of that square root so I can pull it apart.

$s^2\Delta t^2 = ||P_b + V_b \Delta t - P_a||^2$

Because I don’t know a shortcut for this, I’m going to pull the vectors apart.  But for now I’m just going to look at the x component because y and z will look exactly the same and we’ll just add them back in (again because we’re inside a vector magnitude).

$\ldots (P_{b,x} + V_{b,x} \Delta t - P_{a,x})^2 \ldots$

Doing the work of the full squaring, we get this.

$\ldots P_{b,x}^2 + 2 P_{b,x} V_{b,x} \Delta t - 2 P_{b,x}P_{a,x} + V_{b,x}^2 \Delta t^2 - 2 P_{a,x} V_{b,x} \Delta t + P_{a,x}^2 \ldots$

And then pulling together the like terms, we see we’re approaching a quadratic equation on $\Delta t$, which is promising because solving a quadratic equation is straight forward.

$\ldots V_{b,x}^2 \Delta t^2 + 2 (P_{b,x} - P_{a,x}) V_{b,x} \Delta t + P_{b,x}^2 + P_{a,x}^2 - 2P_{a,x}P_{b,x} \ldots$

That’s pretty close, but we can do one more simplification on those last three terms $P_{b,x}^2 + P_{a,x}^2 - 2P_{a,x}P_{b,x}$, that’s just a binomial.  I remembered one shortcut.  Yay!

$\ldots V_{b,x}^2 \Delta t^2 + 2 (P_{b,x} - P_{a,x}) V_{b,x} \Delta t + (P_{b,x} - P_{a,x})^2 \ldots$

Okay now, we need to pull in the other components, y and z.  I’ll do this for each term, so it’s not too messy.

The first term after adding in the other components looks like $(V_{b,x}^2 + V_{b,y}^2 + V_{b,z}^2) \Delta t^2$.  But we recognize that’s the square magnitude of $V_b$, so really we have $||V_b||^2 \Delta t^2$.  This also works for the third term, which becomes $||P_b - P_a||^2$.

The second term,

$2[(P_{b,x} - P_{a,x})V_{b,x}+(P_{b,y} - P_{a,y})V_{b,y}+(P_{b,z} - P_{a,z})V_{b,z}]\Delta t$,

if we look closely, is a dot product between the vector $(P_b - P_a)$ and the vector $V_b$, which becomes $2 (P_b - P_a) \cdot V_b \Delta t$.

So putting it all back together we’ve got,

$s^2\Delta t^2=||V_b||^2\Delta t^2 + 2(P_b - P_a) \cdot V_b \Delta t + ||P_b - P_a||^2$

And then pulling the $s^2\Delta t^2$ over, we finally have a quadratic equation.

$0 = (||V_b||^2 - s^2)\Delta t^2 + 2(P_b - P_a) \cdot V_b \Delta t + ||P_b - P_a||^2$

Finally, we can compute a solution as follows, using

$A=||V_b||^2 - s^2$,

$B=2(P_b - P_a) \cdot V_b$,

$C=||P_b - P_a||^2$.

and plug them into

$\Delta t=\frac{-B\pm \sqrt{B^2 - 4AC}}{2A}$

We’ll get two solutions for $\Delta t$.  One will be negative value of $\Delta t$.  We can see this as the point in the past where if the Target B, shot at the Sniper A then the projectile would have just now arrived.  But what we care about is the future where Sniper A fires at Target B, so we just need to find $\Delta t$ larger than 0.   And we’re able to write out the code.

public float projectileSpeed;

Vector3 CalculateLead () {
Vector3 V = targetVelocity;
Vector3 D = target.position - sniper.position;
float A = V.sqrMagnitude - projectileSpeed * projectileSpeed;
float B = 2 * Vector3.Dot (D, V);
float C = D.sqrMagnitude;
if (A >= 0) {
Debug.LogError ("No solution exists");
return target.position;
} else {
float rt = Mathf.Sqrt (B*B - 4*A*C);
float dt1 = (-B + rt) / (2 * A);
float dt2 = (-B - rt) / (2 * A);
float dt = (dt1 < 0 ? dt2 : dt1);
return target.position + V * dt;
}
}

Here’s an example player showing this code working.

Before I conclude, I want to look at some of the properties that come out of the quadratic solution and how they relate to intuition about the problem.
Going back to the discussion of impossible cases we see that in $A$.  Seeing it in the bottom of the fraction, we know that if $A$ is zero this will blow up.  That happens when Target B’s velocity is the same as the projectile speed.  But looking under the square root we see that if B’s velocity is larger than projectile speed, we get imaginary results, which since we’re looking for a physical solution we want to avoid.

Finally, looking at the numerator of the quadratic solution

$-B \pm \sqrt{B^2 - 4AC}$

Lets make the following substitutions:  $||V_b||$ will be $V$.  $||P_a - P_b||$ will be $D$.  And we’ll replace the dot product in $B$ with $||V|| ||D|| cos \theta$.  And what we wind up with is,

$-2||V|| ||D|| cos \theta \pm \sqrt{4||V||^2 ||D||^2 cos^2 \theta - 4(||V||^2 - s^2)||D||^2}$

Expanding under the square root we get,

$-2||V|| ||D|| cos \theta \pm 2\sqrt{||V||^2 ||D||^2 cos^2 \theta-||V||^2||D||^2+s^2||D||^2}$

And then simplifying,

$-2||V|| ||D|| cos \theta \pm 2\sqrt{||V||^2 ||D||^2 (cos^2 \theta - 1)+s^2||D||^2}$
Because, $sin^2 \theta + cos^2 \theta = 1$, we get,
$-2||V|| ||D|| cos \theta \pm 2\sqrt{s^2||D||^2-||V||^2||D||^2sin^2\theta}$

Which is another binomial under the square root, giving,

$-2||V|| ||D|| cos \theta \pm 2\sqrt{(s||D||-||V|| ||D||sin\theta)^2}$

Removing the square root,

$-2||V|| ||D|| cos \theta \pm 2(s||D||-||V|| ||D||sin\theta)$

Pulling out the $||D||$ and the extra minus sin, and put this back in with the denominator (which lets us get rid of the 2 all together).

$\frac{||D|| (||V|| cos \theta \pm (||V|| sin\theta - s))}{||V||^2 - s^2}$
Seeing this, our earlier intuitions about the problem pop right out.  The $cos\theta$ term represents how fast Target B is moving either directly toward or directly away from Sniper A.  The $sin\theta$ term represents how fast Target B is moving perpendicular to Sniper A.  Pull the math all the way to its final conclusion we’re able to find the patterns that make sense with our original intuition.
I love me some math.