c# - 方向确定算法混合左右

Question

我正在研究一种算法来确定边 BC 相对于边 AB 的方向。

我已经实现了 ArcCos 确定。它几乎可以工作。它正确地确定前进并正确地确定何时需要转弯。但不能正确判断是右转还是左转。

private Direction DetermineDirectionOfV2(Vertex v1, Vertex v2, Vertex v3)
{
    if (v1 == null || v2 == null || v3 == null)
        return Direction.Forward;

    double p12 = v1.distance2D(v2);
    double p13 = v1.distance2D(v3);
    double p23 = v2.distance2D(v3);

    double p12S = p12 * p12;
    double p13S = p13 * p13;
    double p23S = p23 * p23;

    double a = p12S + p13S - p23S;
    double b = 2 * p12 * p13;

    if(b == 0.0)
         return Direction.Forward;

    double angle = Math.Acos(a / b);

    double thresh = 0.1;
    if(angle >= -thresh && angle <= thresh)
    {
        return Direction.Forward;
    }
    else if (angle > 0 && angle < Math.PI)
    {
        return Direction.Right;
    }
    else
    {
        return Direction.Left;
    }

}

我不确定问题出在哪里，但我认为这可能会告诉我角度 ABC，而不是 BC 相对于 AB 的角度。

在左转的情况下，我得到大约 1.1 弧度的角度是错误的。它应该至少 > 3.14 弧度或 -1.1 弧度。

有什么问题吗？

这 3 个顶点:

//x,y,z  (z is unused)
            v1 = new Vertex(0.0f, 0.0f, 0.0f,"");
            v2 = new Vertex(0.0f, -10.0f, 0.0f, "");
            v3 = new Vertex(5.0f, -10.0f, 0.0f, "");

给出一个 0.46 弧度的角，实际上它应该是 4.71238898 弧度，因为从 AB 的角度来看，BC 形成一个 270 度角。

谢谢

例子

如果我们有这样的点:

A


B       C

这应该产生 270 度或 4.7rad，因为如果我们从 A 走到 B，我们将左转到达 C。

Answer 1

首先，你的分子符号错了。余弦规则是

c² = a² + b² - 2*a*b*cos(alpha)

因此，

cos(alpha) = (c² - a² - b²) / (2 * a * b)

在你的术语中意味着:

angle = Math.Acos(p13S - p12S - p23S / (2 * p12 * p13));

很遗憾，这不能解决您的问题。

余弦规则给出了两个导管之间的最小角度。您只处理长度，因此会丢失方向信息。 acos 函数返回 0 到 π 范围内的角度，但您需要从 -π 到 π 的整个圆。

你的问题没有说清楚，但我猜你在 x,y 平面上有二维向量。 (没有引用平面，左右方向就没有意义。)

如果是这样，您可以通过计算 AB 和 BC 的叉积来确定您是左转还是右转。然后检查 z 分量的符号:

private Direction dir(Vertex v1, Vertex v2, Vertex v3)
{
    double ax = v2.x - v1.x;
    double ay = v2.y - v1.y;
    double bx = v3.x - v2.x;
    double by = v3.y - v2.y;

    double z = ax*by - ay*bx;

    if (z > 0.0) return Direction.Left;
    if (z < 0.0) return Direction.Right;
    return Direction.Forward;
}

0.0 的检查当然应该使用合适的阈值进行，就像在您的原始代码中一样。然而，该阈值取决于向量的长度，因为:

|a x b| = |a| * |b| * sin(angle)

此外，前进方向也适用于后退。如果你想区分那些方向，你可以检查标量积是否

ax*bx + ay*by

是正向(Forward)或负向(Backward)。