当前位置：文江博客话题详情

如何计算转弯方向

发布于 2024-09-13 19:35:48 字数 118 浏览 4 评论 0原文

我有一个三个经纬度坐标，组成两条线段 A 到 B 到 C。我还发现了一个函数，可以以 -180 到 180 的方式返回线段 AB 或 BC 的北方位。但是，我很难确定汽车何时从 A 到达 B，应该右转还是左转继续前往 C。

原文

分享到QQ

分享到微博

如果你对这篇内容有疑问，欢迎到本站社区发帖提问参与讨论，获取更多帮助，或者扫码二维码加入 Web 技术交流群。

发布评论

需要登录才能够评论，你可以免费注册一个本站的账号。

蓝戈者 2024-09-20 19:35:48

编辑：以前的答案是错误的。现在这是正确的

public Direction GetDirection(Point a, Point b, Point c)
{
    double theta1 = GetAngle(a, b); 
    double theta2 = GetAngle(b, c);
    double delta = NormalizeAngle(theta2 - theta1);

    if ( delta == 0 )
        return Direction.Straight;
    else if ( delta == Math.PI )
        return Direction.Backwards;
    else if ( delta < Math.PI )
        return Direction.Left;
    else return Direction.Right;
}

private Double GetAngle(Point p1, Point p2)
{
    Double angleFromXAxis = Math.Atan ((p2.Y - p1.Y ) / (p2.X - p1.X ) ); // where y = m * x + K
    return  p2.X - p1.X < 0 ? m + Math.PI : m ); // The will go to the correct Quadrant
}

private Double NormalizeAngle(Double angle)
{
    return angle < 0 ? angle + 2 * Math.PI : angle; //This will make sure angle is [0..2PI]
}

EDIT: Previous answer was wrong. now this is the correct

public Direction GetDirection(Point a, Point b, Point c)
{
    double theta1 = GetAngle(a, b); 
    double theta2 = GetAngle(b, c);
    double delta = NormalizeAngle(theta2 - theta1);

    if ( delta == 0 )
        return Direction.Straight;
    else if ( delta == Math.PI )
        return Direction.Backwards;
    else if ( delta < Math.PI )
        return Direction.Left;
    else return Direction.Right;
}

private Double GetAngle(Point p1, Point p2)
{
    Double angleFromXAxis = Math.Atan ((p2.Y - p1.Y ) / (p2.X - p1.X ) ); // where y = m * x + K
    return  p2.X - p1.X < 0 ? m + Math.PI : m ); // The will go to the correct Quadrant
}

private Double NormalizeAngle(Double angle)
{
    return angle < 0 ? angle + 2 * Math.PI : angle; //This will make sure angle is [0..2PI]
}

回复收藏 0 原文

待＂谢繁草 2024-09-20 19:35:48

已编辑修复超过 180 个问题，现在还支持 U 型转弯。

const int THRESHOLD = 0;
Direction TurnLeftOrRight(Point A, Point B, Point C)
{
    int angle = ToAngle(B,C) - ToAngle(A,B);
    if((angle > THRESHOLD && angle < 180 - THREASHOLD) || angle < -180 - THREASHOLD)
        return Direction.Right;
    else if ((angle < 0 - THREASHOLD && angle > -180 + THREASHOLD) || angle > 180 + THREASHOLD)
        return Direction.Left;
    else if (angle >= 0 - THREASHOLD && angle <= THREASHOLD)
        return Direction.Straight
    else
        return Direction.UTurn;
}

您还可以在左右和海峡之间做公差，只需更改第一个 angle > 即可。 0 到角度> 45 和第二个到角度<; -45

Edited to fix the over 180 issue, also now supports U-Turns.

const int THRESHOLD = 0;
Direction TurnLeftOrRight(Point A, Point B, Point C)
{
    int angle = ToAngle(B,C) - ToAngle(A,B);
    if((angle > THRESHOLD && angle < 180 - THREASHOLD) || angle < -180 - THREASHOLD)
        return Direction.Right;
    else if ((angle < 0 - THREASHOLD && angle > -180 + THREASHOLD) || angle > 180 + THREASHOLD)
        return Direction.Left;
    else if (angle >= 0 - THREASHOLD && angle <= THREASHOLD)
        return Direction.Straight
    else
        return Direction.UTurn;
}

You could also do tolerances between left right and strait just change the first angle > 0 to angle > 45 and the second one to angle < -45

回复收藏 0 原文

猫弦 2024-09-20 19:35:48

我认为如果您使用向量叉积，您的生活会更简单。

虽然严格来说叉积仅针对 3D 向量定义，但对于 2D 向量 p=(px,py) 和 q=(qx,qy)，您可以认为它们叉积 p×q 为 pxqy - pyqx。如果p从q顺时针方向，则后一个数字将为正值；如果p从q逆时针方向，则后一个数字将为负值>。如果p 和q 平行，即指向相同或相反的方向，则该值为零。

就您而言，您正在使用（纬度，经度）。 (x,y) 坐标中的等效项是 (-lon, lat)，因此如果您有两个向量 (lat1,lon1) 和 (lat2,lon2)，则需要计算 (-lon1, lat1)×(-lon2, lat2)，结果为 lat1*lon2-lon1*lat2。

如果这个数字为零，您可以使用点积来判断方向是直线还是直线掉头。

因此，您的代码可能如下所示，假设点和向量以 (lat, lon) 形式编写（如果它们以 x 和 y 表示，则代码会略有不同）：

public Direction GetTurnDirection(Point A, Point B, Point C)
{
    Vector v1 = B - A ;
    Vector v2 = C - B ;
    double cross = v1.lat*v2.lon - v1.lon*v2.lat ;
    if (cross > 0) { return Direction.Left ; } 
    if (cross < 0) { return Direction.Right ; }
    double dot =  v1.lat*v2.lat + v1.lon*v2.lon ;
    if (dot > 0) { return Direction.Straight ; }
    return Direction.UTurn ;
}

I think your life will be simpler if you use the vector cross product.

Although strictly speaking cross product is defined only for 3D vectors, for 2D vectors p=(px,py) and q=(qx,qy), you can think of their cross product p×q as as pxqy - pyqx. This latter number will be positive if p is clockwise from q, and negative if p is counter-clockwise from q. It will be zero if p and q are parallel - i.e. point in the same or opposite directions.

In your case, you are using (lat,lon). The equivalent in (x,y) coordinates is (-lon, lat), so if you have two vectors (lat1,lon1) and (lat2,lon2), you want to calculate (-lon1, lat1)×(-lon2, lat2), which comes out to lat1*lon2-lon1*lat2.

If this number is zero, you can use the dot product to tell whether the direction is straight or a U-turn.

So your code could look like this, assuming Points and Vectors are written in (lat, lon) form (the code would be a little different if they are in x and y):

public Direction GetTurnDirection(Point A, Point B, Point C)
{
    Vector v1 = B - A ;
    Vector v2 = C - B ;
    double cross = v1.lat*v2.lon - v1.lon*v2.lat ;
    if (cross > 0) { return Direction.Left ; } 
    if (cross < 0) { return Direction.Right ; }
    double dot =  v1.lat*v2.lat + v1.lon*v2.lon ;
    if (dot > 0) { return Direction.Straight ; }
    return Direction.UTurn ;
}

回复收藏 0 原文