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 rad，因为从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)

此外，Forward的方向也适用于Backward。如果要区分这些方向，可以检查是否有标量产品

ax*bx + ay*by

为正（向前）或负向（向后）。

方向确定算法左右混合

1 个答案: