Find point on a path in 3d space, given initial and final position and starting velocity vector

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)

Answer 2

Woo, this is interesting.

1. Find the plane that points A, B, and the entity's velocity exist on. Everything will happen on this plane, so now you can think of the problem as 2D, though the math will still be 3D.
2. Find the center point of the circle the entity will be traveling along on the plane. This center point will be perpendicular to the entity's velocity, at a distance of R. There will actually be 2 different places you could place the circle, you'll need to choose based on which is closer to B.
3. Find the two lines from B that are tangential to the circle. The points where these lines touch the circle are your two possibilities for C. Determine which point will allow your entity to hit B when it reaches it and leaves the circle (the other point will bring the entity in exactly the opposite direction).

From this, you should be able to get the position of C, and I'm fairly certain that, if the entity doesn't change speed, the vector should just be the unit vector from C to B times the magnitude of the original velocity vector.

Any or all of this could be wrong. I'm just figuring this from doodling on a piece of paper.

Answer 3

Perhaps this is a shorter version. You could compute C from:

H = O + (r^2)/|BB|)*m

C = H + l*(sqrt(r^2 - |HO|^2)) //this simply applies Pytagora to find out HC magnitude

as a proof of concept:

   cml::vector3f A( 4, 3, 2);
   cml::vector3f B( 1, 5, 0);

   cml::vector3f v( 1, 0, 0);
   float r = 1.4142;

   cml::vector3f n = cml::cross((A-B),v);
   n.normalize();
   cout << "n: " << n << endl;

   cml::vector3f w = cml::cross(n,v);
   w.normalize();
   cout << "w: " << w << endl;

   cml::vector3f O = A + r*w;
   cout << "O: " << O << endl;

   cml::vector3f m = B-O;
   m.normalize();
   cout << "m: " << m << endl;

   cml::vector3f OB = B - O;
   cml::vector3f H = O + m * ( pow(r,2) / OB.length() );
   cout << "H: " << H << endl;

   cml::vector3f l = cml::cross(m,n);;
   l.normalize();
   cout << "l: " << l << endl;

   cml::vector3f OH = H - O;
   cml::vector3f C = H + l * ( sqrt( pow(r,2)-pow(OH.length(),2) ) );
   cout << "C: " << C << endl;