math - 在给定初始和最终位置以及起始速度向量的情况下，在 3d 空间中查找路径上的点

Question

假设我在 3d 空间中有一个实体，并且我知道它的位置向量 (x, y, z) 和它的速度向量(因此它在空间中的方向)。

从已知点A出发，我想分两步到达已知点B:

a) 沿着已知半径 R 的圆形路径转弯，直到点 C

b) 从 C 点直行到终点 B。

速度(标量值)并不重要。速度(矢量)是已知的，所以我想它应该定义圆所在的平面，与其相切，以及 A 和 B 之间的线...

我想知道如何找到 C 的坐标 (x, y, z) 和那里实体的速度向量。

Answer 1

Update: see the working demo!

如果我没理解错的话，你的情况(在包含A、B、v的平面)如下图所示。给定了点 A 和 B，以及矢量 v 和距离 r。你想找到 C 点。

好吧，设向量w = (−v̂_y, v̂_x) 是垂直于v< 的单位向量/强>。那么 O = A + r w。

现在，|C − O| = r 和 (C − B)·(C − O) = 0(其中·是点积)。将这些组合起来得到一个二次方程，您可以求解该方程以找到 C 的两个可能位置。然后选择 (C − B)×(C − O) 符号正确的那个。

(圆心还有第二种选择，O = A − r w，代表顺时针转而不是逆时针转。这给了你另一种可能C .我猜你将不得不使用一些启发式来决定你喜欢哪一个:也许是具有最小∠AOC 的那个。)

---

St0rM 请求帮助以 3D 方式执行此操作(请参阅评论)。这很容易!包含 A、B 和 v 的平面具有法向量 n = (A − B) × v。设 u = n × v 是垂直于 n 和 v 的向量, 并令 w = û(u 方向的单位向量)。

您还需要考虑 C 与 A 位于同一平面的约束:C·n = A.n，并且“右(C − B)×(C − O)的符号”变为“(C − B)×(C − O)·n的正确符号”。

---

求解这个方程组有困难吗？

好吧，如果 (C − B)·(C − O) = 0，则 (C − O + O − B)·(C − O) = 0，因此 (C − O)·(C − O ) + (O − B)·(C − O) = 0，因此 C·(O − B) = O·(O − B) − r²。

您会注意到这是一个平面方程，C·n = A.n 也是如此。将这两个平面相交(see Wikipedia for details — 您可以使用更简单的解决方案，因为这些平面是正交的并且可以很容易地成为正交平面)以获得 C 所在的直线的方程:C = H + λL，例如，其中 L = n×(B-O)。然后用 (C − O)·(C − O) = r² 将其转化为 λ 的二次方程。如果将直线方程重写为 C = H + λL + O，从而消除出现的“- O”，您会发现二次方程会大大简化。

这是使用 numpy 进行向量代数运算的 Python 实现。我相信您能弄清楚如何将其转换为您选择的语言。

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)