Question

Let’s say I have an entity in 3d space and I know its position vector (x, y, z) and its velocity vector (so its orientation in space).

Starting from known point A I want to reach known point B in two steps:

a) turn, following a circular path with a known radius R, until point C

b) go straight from point C to final point B.

Speed (scalar value) is not important. Velocity (vector) is known, so I guess it should define the plane on which the circle resides, being tangent to it, together with the line between A and B…

I want to know how to find coordinates (x, y, z) for C and the velocity vector of the entity being there.

Answer 1

Update: see the working demo!

If I understand correctly, your situation (in the plane containing A, B and v) is as shown in the diagram below. The points A and B are given, as is the vector v and the distance r. You want to find the point C.

Well, let the vector w = (−v̂_y, v̂_x) be a unit vector perpendicular to v. Then O = A + r w.

Now, |C − O| = r and (C − B)·(C − O) = 0 (where · is the dot product). Combine these to get a quadratic equation, which you can solve to find the two possible positions for C. Then pick the one with the right sign for (C − B)×(C − O).

(There’s a second choice for the centre of the circle, O = A − r w, representing turning clockwise instead of anticlockwise. This gives you another possibility for C. I guess you’ll have to use some heuristic to decide which one you prefer: maybe the one with smallest ∠AOC.)

---

St0rM asks for help with doing this in 3D (see comments). That’s easy! The plane containing A, B, and v has normal vector n = (A − B) × v. Let u = n × v be a vector perpendicular to both n and v, and let w = û (the unit vector in the direction of u).

You’ll also need to take into account the constraint that C lies in the same plane as A: C·n = A.n, and “the right sign for (C − B)×(C − O)” becomes “the right sign for (C − B)×(C − O)·n“.

---

Having trouble solving this system of equations?

Well, if (C − B)·(C − O) = 0, then (C − O + O − B)·(C − O) = 0, therefore (C − O)·(C − O) + (O − B)·(C − O) = 0, therefore C·(O − B) = O·(O − B) − r².

You’ll note that this is the equation for a plane, and so is C·n = A.n. Intersect these two planes (see Wikipedia for details — you can use the simpler solution since the planes are orthogonal and can easily be made orthonormal) to get the equation of a line on which C lies: C = H + λL, say, where L = n×(B − O). Then use (C − O)·(C − O) = r² to turn this into a quadratic equation in λ. You’ll find that the quadratic equation simplifies quite a bit if you rewrite the equation of the line as C = H + λL + O so that occurrences of “− O” disappear.

Here’s an implementation in Python using numpy to do the vector algebra. I’m sure you can figure out how to convert this to the language of your choice.

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*L + O
        H = dot(A, n) * n + (r**2 / norm(BB)) * m
        L = cross(n, m)
        # |C - O| = r**2 gives quadratic eqn in l with coefficients a=1, b=0, c.
        c = dot(H, H) - r**2
        disc = - 4 * c          # Discriminant of quadratic eqn.
        if disc < 0:
            continue            # No tangents (B inside circle).
        elif disc == 0:         # One tangent (B on circle).
            C = H + O
            yield (sense, C)
        else:                   # Two tangents (B outside circle)
            for sign in (-1, +1):
                l = sign * math.sqrt(disc) / 2
                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)