Turning circles

,

Over on stackoverflow.com, user St0rM asks this question (paraphrased slightly):

Let’s say I have an entity in 3D space and I know its position \(A\) and its heading \(\vec v\). Starting from \(A\) I want to reach known point \(B\) in two steps:

  1. turn, following a circular path with a known radius \(r\), until point \(C\);
  2. go straight from point \(C\) to final point \(B\).

I want to know how to find \(C\).

Figure 1 shows the intended motion. Here \(O\) is the centre of the turning circle.

It’s important to bear in mind that the motion in figure 1 might not be possible. In particular, if \(B\) is inside the turning circle, then there’s no suitable point \(C\). However, you may be able to turn the other way, as in figure 2.

How should we go about solving this? A problem-solving heuristic to bear in mind is that this kind of geometric problem is almost always easiest to solve if one keeps the problem in the realm of vector algebra as long as possible, switching to Cartesian coordinates only as a last resort, if at all. Geometric objects like lines and circles, and geometric operations like intersections, are generally more convenient and concise when represented in vector form.

First we need to find the plane in which everything takes place. In vector geometry, a plane is represented by an equation of the form \(P·\vec n = k\) where \(P\) is a point on the plane, \(\vec n\) is a normal vector to the plane, and \(k\) is a constant. If \(\vec n\) is a unit vector then \(k\) is the distance of the plane from the origin. (It’s straightforward to prove this. For suppose \(P\) and \(Q\) are two points on a plane. Then \(P − Q\) is perpendicular to \(\vec n\), so \((P − Q)·\vec n = 0\), and therefore \(P·\vec n = Q·\vec n =\) some constant, \(k\), say.)

In this problem, the plane contains the line \(AB\) and the heading \(\vec v\), so let \(\vec n = \widehat{(A − B)×\vec v}\) be a unit vector perpendicular to both \(A − B\) and \(\vec v\), and so normal to the plane as required. Then the equation of the plane is \(P·\vec n = A·\vec n\) (or \(P·\vec n = B·\vec n\), it doesn’t matter which).

Now we can find \(O\) and \(O’\). Let \(\vec w = \widehat{\vec n × \vec v}\) be a unit vector perpendicular to both \(\vec n\) and \(\vec v\). Then \(O\) and \(O’\) are \(A + r\vec w\) and \(A − r\vec w\) in some order.

Now \(C\) is one of the two points where a line from \(B\) meets the circle with centre \(O\) and radius \(r\) at a tangent. Figure 3 shows the two tangents: the tangent drawn in blue is not a solution to the problem, because the motion would have to reverse to use this line. We’ll work out how to tell these tangents apart later. Now there are three things we know about the point \(C\):

  1. It’s on the circle: \(|C − O| = r\), that is, \((C − O)·(C − O) = r^2\).
  2. The lines \(OC\) and \(BC\) are perpendicular, that is, \((C − O)·(C − B) = 0\).
  3. It lies in the plane: \(C·\vec n = A·\vec n\).

We can rewrite (b) as \((C − O)·(C − O + O − B) = 0\), and expand this product to get $$ (C − O)·(C − O) + (C − O)·(O − B) = 0. $$ Substituting (a) for the first dot product gives $$ \eqalign{& r^2 + (C − O)·(O − B) = 0 \cr ∴ & (C − O)·(O − B) = − r^2 \cr ∴ & (C − O)·(B − O) = r^2.} $$ This should be recognizable as a plane equation, with \(B − O\) as the normal to the plane. It should be evident that this plane is perpendicular to the plane with normal \(\vec n\), and the two planes intersect in the line drawn in red in figure 4. Let \(\vec m = \widehat{B − O}\) be the unit normal to this plane, and divide the last equation though by \(|B − O|\) to get it in the form $$ (C − O)·\vec m = {r^2 \over |B − O|}. $$ The plane normals \(\vec n\) and \(\vec m\) are two orthogonal unit vectors. This means that when we intersect the two planes we are in the easy orthonormal case, and we find that the red line has equation \(C − O = H + λL\), where $$ H = (A·\vec n)\vec n + {r^2 \over |O − B|}\vec m\ {\rm and}\ L = \vec n × \vec m $$ Geometrically, \(H + O\) is the point where the altitude from \(C\) meets the line \(OB\) at a right angle. Substituting \(C − O = H + λL\) into (a) gives $$ \eqalign{ & (H + λL)·(H + λL) = r^2 \cr ∴& λ^2 L·L + λ2L·H + H·H − r^2 = 0} $$ Since \(L·L = 1\) and \(L·H = 0\), this simplifies to \(λ^2 + H·H − r^2 = 0\) and so \(λ = \sqrt{r^2 − H·H}\).

When the discriminant \(r^2 − H·H\) is less than zero, there are no solutions for \(C\) since \(B\) is inside the turning circle. When \(r^2 − H·H\) is zero, \(B\) is on the turning circle, so \(C = B\). Finally, if \(r^2 − H·H\) is greater than zero, there are two solutions for \(C\). Which one is right? Well, it’s the one where the sense of turning \(O\rightarrow C\rightarrow B\) is the same as the sense of turning \(A\rightarrow C\rightarrow B\). If the latter is anticlockwise, we need \((C − B)×(C − O)\) to be pointing out of the same side of the plane as \(\vec n\). That is, \((C − B)×(C − O)·\vec n > 0\). And if the turning circle is clockwise, \((C − B)×(C − O)\) needs to be pointing out of the opposite side of the plane to \(\vec n\). (You can check this with careful use of the right-hand rule for vector cross products. Or do what I did and implement it both ways round and see which one is right.)

Here’s an implementation in Python, using numpy to do the vector algebra. Note that we never need to descend to Cartesian coordinates at all.

# -*- encoding: utf-8 -*-

import math
from numpy import cross, dot
from numpy.linalg import norm

def unit(v):
    """Return a unit vector in the same direction as v."""
    return v / norm(v)

def turnpoints(A, B, v, r):
    """Generate possible turning instructions for a path from A to B
that starts out in direction v, turns through part of a circle of radius
r until it reaches a point C (to be determined), then heads straight for
B. Return each instruction in the form (sense, C) where sense is -1 for
clockwise and +1 for anticlockwise."""
    n = unit(cross(A - B, v))   # Unit normal to plane containing A, B, v.
    w = unit(cross(n, v))       # Unit normal to v lying in that plane.
    for sense in (-1, +1):      # Turn clockwise or anticlockwise?
        O = A + sense * r * w   # Centre of turning circle.
        BB = B - O
        m = unit(BB)
        # C lies on the line H + λ*L + O
        H = dot(A, n) * n + (r**2 / norm(BB)) * m
        L = cross(n, m)
        l2 = r**2 - dot(H, H)   # λ-squared
        if l2 < 0:
            continue            # No tangents (B inside circle).
        elif l2 == 0:           # One tangent (B on circle).
            yield sense, B
        else:                   # Two tangents (B outside circle)
            for sign in (-1, +1):
                l = sign * math.sqrt(l2)
                C = H + l * L + O
                # Only one choice for C is correct (the other involves
                # reversing direction).
                if dot(cross(C - B, C - O), n) * sense > 0:
                    yield sense, C

And here’s a implementation in Javascript for you to experiment with. This demo is two-dimensional only, because it needs to be able to draw the results onto a two-dimensional canvas. Internally the code is actually using three-dimensional vectors (using Sylvester to do the vector algebra). The points labelled \(H\) are actually \(H+O\) but it looks cluttered to use the full label. I leave it as an exercise for the reader to find the most suggestive configuration.

\(A_x\) \(A_y\) \(B_x\) \(B_y\) \(v_x\) \(v_y\) \(r\)