Как рассчитать направление поворота

Question

У меня есть три широтные координаты, которые составляют два отрезка линии от А до В до С. Я также нашел функцию, которая может возвращать направление на север отрезка линии АВ или ВС от -180 до 180 способом.Однако у меня возникают проблемы с определением того, когда автомобиль движется от А к В, если он поворачивает направо или налево, чтобы продолжить движение до С.

Answer 1

РЕДАКТИРОВАТЬ: Предыдущий ответ был неправильным. теперь это правильный

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]
}

Answer 2

Я думаю, что ваша жизнь будет проще, если вы используете векторное перекрестное произведение .

Хотя строго говоря, перекрестное произведение определено только для трехмерных векторов, для двухмерных векторов p = (px, py) и q = (qx, qy), вы можете думать об их перекрестном произведении p × q как px qy - py qx.Последнее число будет положительным, если p по часовой стрелке от q , и отрицательным, если p против часовой стрелки от q .Это будет ноль, если p и q параллельны - то есть указывают в одинаковом или противоположном направлении.

В вашем случае вы используете (широта, долгота),Эквивалент в (x, y) координатах равен (-lon, lat), поэтому, если у вас есть два вектора (lat1, lon1) и (lat2, lon2), вы хотите вычислить (-lon1, lat1) × (-lon2,lat2), который выходит к lat1 * lon2-lon1 * lat2.

Если это число равно нулю, вы можете использовать dot product , чтобы определить, является ли направление прямым или U-turn.

Таким образом, ваш код может выглядеть следующим образом, если предположить, что Точки и Векторы записаны в виде (широта, долгота) (код был бы немного другим, если бы они были в х и у):

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 ;
}

Answer 3

Отредактировано для исправления проблемы более 180, теперь также поддерживает развороты.

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 на angle > 45, а второй на angle < -45

Answer 4

Если AB - это азимут В от A, а BC - от C от B, то угол поворота остается остаточным (BC-AB, 360.0);(принимая градусы).Если это положительно, поворот направо.В вашем примере остаток (BC-AB, 360.0) - это остаток (271 360) = -89.